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ABSTRACT 



The recognition of underwater objects and obstacles by sonar has been explored in 
many forms, particularly through the use of high-resolution imaging sonar systems. This 
work explores a method of providing real-time obstacle avoidance and navigational 
position updating for an Autonomous Underwater Vehicle (AUV) by applying regression 
analysis and geometric interpretation to sonar range data obtained from a low-cost, low- 
resolution, fixed-beam sonar. The algorithm utilized by this method first develops a least- 
squares fit for sonar range data in a 2-D manner. The parameters developed by this method 
are then compared to an environmental model for position identification. If no match is 
achieved, then by applying the known geometry of the acoustic signal, an estimate for a 3- 
D surface is derived. This derived 3-D surface is then added to the environmental model to 
enable accurate path planning and post-mission analysis information. This method is 
currently implemented on an operational AUV operating in a well-defined orthogonal 
environment at NPS. The paper also discusses the simulation of the sonar systems using a 
ray tracing technique in a real-time dynamic graphical simulation implemented on a Silicon 
Graphics IRIS workstation. 
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I. INTRODUCTION 



A. MOTIVATION FOR AUV RESEARCH 

Remotely operated air and land vehicles have been developed to extend the range of 
sensors or weapons of military units. The Israeli Air Force utilized remotely piloted 
vehicles (RPVs) for reconnaissance and electronic intelligence collection prior to the 
successful air strikes in the Bekka Valley in 1982. This utilization of RPVs as tactical 
probes resulted in zero losses to the Israeli Air Force, and near-total destruction of the 
Syrian Air Force [Ref. 1 ]. 

Current Maritime Strategy emphasizes the U.S. Navy’s role in a forward area strategy 
conducting operations in or near enemy waters [Ref. 2], Given the Soviet Navy’s “bastion” 
defense strategy, utilizing multiple layers of defensive units to protect strategic missile 
submarines, an underwater vehicle could serve as an effective tactical probe. Due to the 
long-range aspect of such a mission and the presence of numerous enemy units, the 
presence of a “mother” platform providing a radio control link is not feasible. An 
autonomous underwater vehicle (AUV) with onboard sensors and control units, capable of 
carrying out a preplanned mission without external guidance, has been shown to be of 
potential military value and cost effectiveness [Ref. 1]. 

While the attention given to Soviet and Warsaw Pact military strategies may have 
lessened due to the decline of Communism in Eastern Europe and the Soviet Union, events 
in and around the Persian Gulf have validated the need for truly autonomous vehicles to 
assist in effecting the Maritime Strategy. 
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Numerous missions are conceivable for an AUV. Some typical missions might include 
the following: 

• Tactical probe 

• Covert surveillance 

• Mine warfare 

• Weapons delivery 

• Underwater terrain mapping. 

Equipped with the appropriate sensors and manipulators, an AUV could carry out these 
missions without exposing tactical units or personnel to danger, and for a lower cost. 

B. AUV CONCEPT 

In the simplest form, an AUV is an unmanned submersible vehicle with onboard 
systems and sub-systems that provide motive power, motion control, navigation, obstacle 
detection and collision avoidance. To be truly autonomous, the vehicle should be able to 
execute a planned mission by controlling and monitoring the onboard systems without any 
external input. It should be able to replan its mission in the event of internal anomalies, such 
as sub-system degradation, and it should have the capability of replanning its path to avoid 
previously unknown obstacles [Ref. 3J. While the above qualities outline the minimum re- 
quirements for a simple AUV, a realistic mission-capable AUV would necessarily carry 
additional, possibly specialized, mission-dependent sub-systems and sensors. The Naval 
Postgraduate School (NPS) AUV II is designed to model the simple AUV concept. 

C. OBSTACLE AVOIDANCE 

The concept of obstacle avoidance for an AUV entails more than simply turning the 
vehicle in a predefined manner to avoid a collision. In the context of an AUV conducting 
a predefined mission, the concept of obstacle avoidance carries the implicit notion that the 
AUV will attempt to continue its mission by replanning its path around the obstacle. This 
further implies two additional capabilities, the ability to consult a stored model of the en- 
vironment, and the ability to detect, recognize, and quantify previously unknown obstacles 
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so that they may be added to the stored model [Ref. 3J. This can be simplified into the fol- 
lowing steps: 

• Detect the obstacle. 

• Perform a safety maneuver to avoid the obstacle. 

• Perform other maneuvers to extract obstacle features. 

• Add the obstacle to the stored environmental model. 

• Replan the path to avoid the obstacle while continuing the mission. 



This thesis will address all but the final step. Work on the path replanning problem has been 
addressed by other NPS AUV II project team members [Ref. 4], 

D. OBJECTIVES 

This thesis will address the following research questions: 

• How can data from ultrasonic sensors be best utilized to recognize obstacles 
during operation of the NPS AUV II? 

• What is the optimal configuration for ultrasonic sensors on the AUV II to 
provide obstacle detection and terrain data collection for post-mission 
analysis? 

• What type of motion algorithm will best provide collision avoidance and 
obstacle feature extraction? 

• How can sensor data be utilized in post-mission analysis to generate a 3-D 
terrain model? 



This research was designed to move the NPS AUV II project into its next phase of devel- 
opment by providing a stable test platform capable of maintaining a collision-free path 
while conducting missions in its current environment, the NPS swimming pool. At the out- 
set of this research, the AUV II had no sensors installed and was capable of performing only 
simple, open-loop missions in the swimming pool. 

E. THESIS ORGANIZATION 

Subsequent chapters of this work will address the research questions posed above. 
Chapter II describes the NPS AUV II in terms of its physical characteristics as well as its 
control and software hierarchy. This section describes previous work done on this project. 
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Chapter II also examines the most current work on sonars and AUVs. Chapter III provides 
the theoretical framework for sonar system operation, the physical and electrical character- 
istics of the sonar chosen for the NPS AUV II, and some of the problems inherent with these 
systems and their installation. 

Chapter IV discusses the extraction of linear features of obstacles by means of a least 
squares fit algorithm. The algorithm and the particular tests performed on sonar range 
inputs are examined, also. Chapter V covers the utilization of the information developed by 
linear feature extraction by the collision avoidance system and other processes. 
Experimental results are presented in Chapter VI. Pre-mission simulation and post-mission 
analysis and 3-D display of mission data is discussed in Chapter VII. Finally, conclusions 
regarding the viability of this particular approach to collision avoidance and 
recommendations for further study are presented in Chapter VIII. 
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II. NPS AUV PROJECT AND RELATED AUV WORK 



The Naval Postgraduate School’s AUV project was started in 1987, and involves 
personnel from the Mechanical Engineering, Computer Science, and Electrical 
Engineering departments. The project has evolved through design and feasibility studies, 
the construction and testing of the NPS AUV I radio-controlled model vehicle, the 
construction of the AUV II vehicle, and its ongoing testing and development. [Refs. 5, 6, 7] 

A. THE NPS AUV II VEHICLE 
1. Vehicle Characteristics 

The basic layout of equipment for the AUV II vehicle is illustrated in Figure 2-1. 
The vehicle design has been detailed by Good [Ref. 8]. The main vehicle body is 
constructed as an aluminum box with a beam of 16 inches, a height of 10 inches, and a 
length of 72 inches. The nose cone is constructed of fiberglass and extends the overall 
vehicle length to 92 inches. The vehicle uses fixed ballast and displaces approximately 390 
pounds. Vehicle control is provided by eight independently driven control surfaces, four 
tunnel thrusters, and two main drive motors. The counter-rotating 24 volt DC drive motors 
power four-inch propellers and provide one-eighth horsepower each, driving the AUV at a 
maximum speed of about two knots (3 feet per second). The control surfaces provide a high 
degree of maneuverability, with a minimum turning diameter of approximately 20 ft., less 
than three ship lengths. All of the installed systems are powered by lead-acid gel batteries 
capable of providing power for up to two and one-half hours. 

The control and guidance software processes run on a GESPAC MPU 20HF processor 
with a Motorola 68020 CPU and 68881 math coprocessor running at 16 MHz. The system 
has 2.5 Mb of RAM and runs the OS-9 multi-tasking operating system. Input and output 
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between the CPU and the installed systems is routed through two GESDAC-2B 8-channel 12 bit 
Digital-to-Analog/Analog-to-Digital (DA/AD) converter cards and a GESPIA-3A parallel 
interlace board. Serial communications with external systems is achieved via a 2400 bps modem. 
The navigation system sensor suite includes a flux gate compass and directional gyroscope, a 
vertical gyroscope system, and a three axis rate gyroscope system with translational 
accelerometers. A paddlewheel speed sensor is installed in the nose, along with the four sonar 
transducers. 




Figure 2-1. NPS AUV II Vehicle Component Layout. 
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2. Software Hierarchy 



A block diagram illustrating the interaction of systems and processes is given in 
Figure 2-2 [Ref. 9]. The dataflow diagram in Figure 2-3 represents the software 
instantiation of the processes shown in Figure 2-2. The individual data items are further 
described in the Data Dictionary located in Appendix A. The mission plan and the 
particular environmental database for the operating area are downloaded to the AUV from 
the mission support system, running on a GRIDCASE 386 laptop computer, via the 
modem. Referring now to Figure 2-2, the mission executor oversees the mission execution 
by providing geographic waypoints and tasks to the guidance system. The guidance system 
provides desired vehicle postures, (x,y, z, 0), as heading, speed, and depth commands to 
the autopilot system. The autopilot, updated by the navigation system, controls vehicle 
systems to achieve the desired postures. Vehicle systems are monitored for possible 
problems, such as the loss of a control servo, that might necessitate mission replanning. 

The sonar systems provide range data that are used to detect obstacles and to 
develop obstacle features. The pattern recognition system attempts to match obstacle 
features with known obstacles in the database in order to provide position updates to the 
navigation system. Those obstacles that are not found in the database are added to the 
model, and the obstacle avoidance decision maker is signalled to take possible evasive 
measures. Details of the sonar processes are discussed in Chapter V. Additionally, the 
mission replanner is signalled to develop a new path utilizing the updated environmental 
database model. All of the processes running on the GESPAC are coded in C. 
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Figure 2-2. NPS AUV II System Block Diagram. 
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Figure 2-3. AUV Dataflow Diagram. 



B. RELATED WORK 



1. Sonar Range Finding 

There are a few research reports on linear fitting on radial range data from ultrasonic 
sensors in autonomous land vehicle control. Crowley proposed the recursive linear fitting 
algorithm to find a linear segment among radial data [Ref. 10]. Drumheller proposed an 
iterative endpoint fit for the same purpose [Ref. 1 1], Several reports stress the uncertainty 
and ambiguity of sonar range data [Refs. 12, 13, 14]. The method of range data fitting 
detailed in Chapter IV was tested on the autonomous mobile robot Yamabico-1 1 which was 
developed at the University of California at Santa Barbara and at the Naval Postgraduate 
School (NPS) by Kanayama [Ref. 15]. 

2. AUV Sonar Research 

One other AUV project has utilized a similar sonar system, and used range data 
slope information provided by the sensors to assess targets [Ref. 16]. Most work in this area 
has utilized high resolution sector-scanning, or multi-beam type sonars and image 
processing algorithms for obstacle recognition [Refs. 17, 18] and position estimation [Ref. 
19]. 
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III. ULTRASONIC TRANSDUCER PROPERTIES 



AND INSTALLATION 



A. DATASONICS PSA-900 PROGRAMMABLE SONAR ALTIMETER 
1. Physical and Electrical Characteristics 

The ultrasonic transducer chosen for use on the NPS AUV project is the PSA-900 
Programmable Sonar Altimeter manufactured by Datasonics, Inc. Each transducer system 
consists of a transducer head, a microprocessor-based control system, and associated 
connecting cables. Specifications for the PSA-900 are listed in Table 1. Each transducer 
head measures 2.25 inches in diameter, and 1 to 2 inches in length. 



TABLE 1-1. DATASONICS PSA-900 SPECIFICATIONS 



Operating Frequency: 175/200/223 kHz (fixed) 

Beam Pattern: 10°, conical 



Pulse Length: 
Repetition Rate: 



350 (isecs 

User-selectable (10/1/0.1 pps) 
User-selectable (30 / 300 meters) 



Range: 



Resolution: 



1 cm @ 30 m range / 10 cm @ 300 m range 
+ 0.25% of full scale range 
0 - 10 v DC, proportional to full scale range 



Accuracy: 
Range Output: 



Power Requirement: 15 - 28 v @ 100 mA DC 



[Ref. 20] 



2. Theory of Operation 

A sonar operates by emitting acoustic energy at a specified frequency and duration. 
Sonar range is determined by timing the sound pulse as it travels from the transducer, 
strikes an object, and then returns to the transducer. If the speed of sound through the water 
( Cj ) is known, then the range (d) can be determined from the following formula: 

(C ± AC ) (t ± At) 

d = ^ (Eq3.i) 

The nominal speed of sound (C s ) in salt water with salinity of 35%o is 1500 meters per sec- 
ond. The A C s factor represents changes in sound velocity due to environmental factors 
such as temperature, pressure, salinity and depth. Of these factors, temperature change con- 
tributes the most to sound velocity change, with as much as a five percent net increase or 
decrease in sound velocity. To compensate for temperature related changes in sound veloc- 
ity, the PSA-900 includes a temperature sensor that enables it to make automatic 
adjustments based on actual temperature readings.The A t factor in (Eq 3. 1) is the error fac- 
tor in determining the actual time elapsed since the sound pulse was generated and is 
referred to as jitter. For the PSA-900 sonar this jitter error can be as small as 5 microsec- 
onds, or approximately 0.4 cm total distance [Ref. 20J. 

The ability of a sonar to detect an echo is determined by the initial pulse strength, 
the size and type of the target, the distance to the target, and other factors related to noise. 
Expressed in terms of the detection threshold ( DT ), or the ability of the sonar to just detect 
a target, the active sonar equation is 

DT = SL- 2 TL + TS- ( NL - Dl) (Eq 3.2) 

Here, SL is the signal level of the original pulse, 2TL is the two-way transmission path loss, 
NL is the background noise, and Dl is the directivity index of the receiver. All terms are 
expressed in units of decibels relative to the standard reference intensity of a lqPa plane 
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wave. The transmission loss (TL) is due to spherical spreading of the sound energy and ab- 
sorption of sound energy by particles suspended in the water. For the active sonar this is 
expressed as 

TL = 201og2r + 2ar (Eq3.3) 

where a is the attenuation coefficient of sound in water at the frequency in use and r is the 
length of the transmission path |Ref. 21, pp. 19-23, 110-111]. With the AUV operating in 
the swimming pool environment, the noise term ( NL ) can be disregarded. To compensate 
for the transmission loss expressed in (Eq 3.3), the PSA-900 utilizes a time varying gain 
(TVG) amplifier to enhance signal detection capability. This TVG circuitry increases the 
gain of the receiver amplifier as a function of time to ensure that weak echoes are detected 
[Ref. 20]. 

3. Side Lobe Effects 

The PSA-900 transducer is a circular plane type and produces a main acoustic beam 
lobe that extends approximately 15 degrees around the centerline, producing a circular 
pattern. Additionally, the transducer produces significant side lobes at approximately 25 
and 50 degrees around the centerline. Any objects in the area of the side lobes can produce 
echoes as though the object were along the centerline of the sonar. These false returns can 
produce erroneous results unless corrected or eliminated. 

Since it is impossible to alter the physical characteristics of the sonar beam, the 
method of attempting to eliminate the side lobe returns focused on the design of a shield to 
be placed on each transducer. The shield is in the form of a conic section, measuring 2.5 in. 
in length, with an angle of 5° on either side of the centerline. This shield deflects the initial 
side lobe pattern and blocks sonar returns outside of 5° of the sonar’s extended centerline. 
Figure 3-1 illustrates the effect of the shield. 



13 



Shield 



Transducer 



1 




1 50 J 


I 5.2 m 






' 5° 


[5.2 m 



meters 



Figure 3-1. 2-D View of the Shielded Sonar Beam. 

4. Interference Problems 

Preliminary investigations using these sonars revealed that simultaneous operation 
of two sonar systems, both operating in a self-keying mode with different frequencies, 
could result in interference and erroneous range readings. Previous tests with both systems 
keyed simultaneously via the external keying signal input showed that mutual interference 
should not be a problem [Ref. 22, pp. 28-33]. Post-mission data analysis of tests conducted 
with two sonars, mounted orthogonally in the AUV’s nose and keyed simultaneously, 
revealed no evidence of mutual interference. 

5. Sonar Board Averaging 

The PSA-900 processor maintains a sliding window average consisting of the last 
four ranges. This average is used to determine the validity of range signals, with signals 
differing from the average by more than 10% of the maximum selected range (3 meters) 
considered to be in error. Due to this on-board averaging, when a different sonar transducer 
is selected for use with the same board, a number of pings must be conducted to “wash out” 
the effects of the previous average. Test results revealed that a minimum of 15 pings must 
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be generated before the new average settled at the correct range (Figure 3-2). With a ping 
rate of 10 Hz, this reduces the effective data acquisition rate to less than 0.3 Hz for each 
transducer operating on the same board. While this rate is acceptable for some missions, it 
is too low for purposes of control or obstacle avoidance. 

6. Noise Filtering 

Initial tests conducted with the sonars installed in the AUV showed that the signals 
provided by the sonars contained a noticeable amount of spurious noise. Examination of a 
number of sets of test data led to the observation that the noise tended to present itself as a 
transient spike indicating a range shorter than the actual range. Further, each spike’s first 
transition duration (rise time) was very short, with the first data point in the spike being 15 
- 20 units less than the previous valid data point. The spikes average duration was 1 second. 
At a 10 Hz data rate, this means that ten consecutive data points may be invalid. An 
enlarged section of an unfiltered data set from the bottom sonar is seen in Figure 3-3. 

The analysis of the noise present in the signals led to the development of a digital filter 
that screens out points more than 15 units away from the current average value. Points that 
pass this screen are included in the updated moving average, consisting of the ten most 
recent valid points. In order to prevent the loss of possibly valid data representing a 
significant change in a feature’s topography, points screened out are saved in a buffer. If a 
sequence of ten points are screened out, then those ten points are used to develop a new 
average, and the buffer is cleared. When dealing with the forward sonar, the screening must 
allow for changes in the range signal due to the forward motion of the AUV at its current 
speed. Typically, this results in a screen of 25 - 30 units, rather than the 20 used for the side 
and bottom sonars. The data set seen in Figure 3-3 is shown in Figure 3-4 after filtering. 
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Figure 3-2. Illustration of Sonar Board Averaging. One sonar board has been 
switched between two transducers every 2 seconds (20 pings). Ranges for one 
head are shown by dots, ranges for the second head are shown with asterisks. 
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Figure 3-3. Unfiltered Range Data Set. Data from the bottom so- 
nar. Range is in interface units, where 1 unit = 0.023 ft. 



17 




Figure 3-4. Filtered Range Data Set. Filtered values are shown 
with “x’s”, original data (dots) from Figure 3-3 is also shown for 
comparison. Range is in interface units, where 1 unit = 0.023 ft. 



B. SONAR INSTALLATION 
1. AUV Nose Mount 

The four sonar transducers, with associated cones, currently installed in the AUV II 
are affixed to an aluminum mounting bracket, so that they are mutually orthogonal. There 
is a sonar oriented directly forward, directly downward, to the left, and to the right of the 
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Al'V body. Since the fiberglass nose is acoustical!) opaque for the sonar frcquenc tes in 
holes were cut in the nose to expose the openings of the shield cones. A thin plastic sheet, 
acoustically transparent, was placed over the openings to maintain the hydrodynamic prop- 
erties of the nose. The watertight cables for the sonars are connected to a watertight 
connection, mounted on the forward bulkhead of the AL'V, which connects to the sonar pro- 
cessing boards. The sonar mount is shown in Figure 3-5. 




Figure 3-5. AUV II Sonar Installation. Shield has been removed from forward 
sonar to show transducer. Watertight connection is at upper left. 
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2. Computer Interface 

Due to power supply limitations, there are currently two sonar processing boards 
installed in the AUV II. The four transducers are connected to the two boards through a set 
of four microswitches, so that one board serves two transducers (Figure 3-6). Each 
microswitch is software controlled to enable the selection of a pair of sonar transducers for 
use. The switch interface to the CPU is via the GESPIA-3A parallel interface board. 
Additionally, the external keying signal is provided to each sonar board via the GESPIA- 
3A. To effect a controlled sonar ping, the appropriate pair of transducers are selected for 
connection to the sonar boards, and then the boards are keyed via the parallel interface. This 
simultaneous keying precludes the crosstalk problems discussed earlier in this chapter. 

The analog range signal (0-10 v DC) provided by the sonar processor is interfaced 
to the CPU via the GESDAC-2B analog-to-digital converter board. This interface provides 
a resolution of 4096 units, with programmable gain control. The sonar installation uses 
unity gain, with full scale representing the maximum selected range (4096 units = 10 volts 
= 30 meters). 
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Transducers 



Figure 3-6. Sonar-Computer Interface Diagram. 



21 






IV. EXTRACTION OF LINEAR FEATURES 
OF OBSTACLES 



Regression analysis is used in many applications to find linear approximations for sets 
of discrete data points. In the case of underwater obstacles and the AUV sonar, data points 
generated by sonar returns from obstacles may be used to generate linear features of the ob- 
stacles that can be used to describe the obstacles in terms of the environmental model. This 
linear feature extraction enables the system to perform pattern matching with the environ- 
mental database to allow navigational position updating, or, in the case of a previously 
unknown obstacle, the obstacle can be added to the environmental database. 

This section discusses the application of the least squares fit method to sonar data and 
the extraction of linear features of obstacles. These linear features are then described in 
terms of the AUV’s world environmental model to allow pattern matching or database up- 
date. [Ref. 23J 

A. LEAST SQUARES FIT METHOD 
1. Coordinate Transformation 

In determining global coordinates for the data points, we use the AUV’s dead 
reckoning (DR) position (x s , y s ) and heading orientation, \jr, with respect to the global 

system, and the sensor’s orientation with respect to the AUV body (Figure 4-1 ). For the side 
and forward sonars, the effects due to roll and pitch are negligible and can be ignored due 
to the inherent stability of the AUV in these axes. Similarly, the bottom sonar range is 
affected only by the pitch angle, with effects due to roll being minimal, and the range being 
independent of heading. Note that the heading angle, \j/, is measured in a clockwise fashion 
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Figure 4-1. World Coordinate Conversion. Diagram illustrates parameters for 
a range return from the left sonar. 
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around the z axis by a gyroscope. The DR position is determined by 



x' s = x s + At x u x cos (y) 



(Eq 4.1) 



y' s = y s + At x u x sin (y) 



(Eq 4.2) 



where At = 0.1 secs and u is an estimate of the velocity along the vehicle’s longitudinal 

velocity. This estimate for the velocity is a primary source of DR error. Coordinates for the 

data point (p r , p.,) from the left sonar are generated as follows: 
y 



2. Linear Regression Principles 

We first discuss how to extract a linear feature from a set of point data by the least 
squares fit method. The linear feature is the simplest one among all the abstract geometric 
features, and is easily obtained by range sensors from an orthogonal world. Suppose a set 
R of positions for an envelope of an object in a plane is given from a range sensor. 



p x = x s + range x cos ( \|/ — n/2) 



(Eq 4.3) 



P y = )' s + range x sin (V -n/2) . 



(Eq 4.4) 



R = «*, y ,•)!/= / n) . 



(Eq 4.5) 



The moments mn. of R are defined as 



n 




i= 1 



(Eq 4.6) 



Notice that /« 00 = n . The centroid C of R is given by 




(Eq 4.7) 



The secondary moments around the centroid are given by 




(Eq 4.8) 
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n 



M n~ Z ( x , ~ ^ (>«■" V = m n~ ( 



) 



(Eq 4.9) 




2 



(Eq 4.10) 



We adopt the parametric representation (r, a) of a line with constants r and a. If a point 
p= ( x , y) satisfies an equation 



then the point p is on a line L whose normal has an orientation a and whose distance from 
the origin is r (Figure 4-2). This method has an advantage in expressing lines that are per- 
pendicular to the X axis. The point-slope method, where y = mx + b, is incapable of 
representing such a case {m = °°, b is undefined). The residual of point p i = (x-, >•) and 
the line L = ( r , a) is x-cosa + y- sina - r . Therefore, the sum of the squares of all re- 
siduals is 



The line which best fits the set of points is supposed to minimize S. Thus the optimum line 
(r, a) must satisfy 



r = xcosa + ysina (-rc/2 < a < n/2) 



(Eq 4.1 1) 



n 




(Eq 4.12) 



dS _ dS_ 
dr da 



(Eq 4.13) 
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Figure 4-2. Representation of a Line. The distance from 
the point p to L is the residual. 



Thus, 



and 



as 

dr 



n 

= 2^ (r-A ( cosa- v-sina) 
i = l 



f ” 




\ 


( " \ \ 


M 

1 


(2>< 


cosa- 


Z>’« sina 


^i=l 


M = 1 - 


) 


^ = i J ) 



= 2 (rniQQ — A/ijoCosa-mo^ina) 



= 0 



m 10 , W 01 . 

r = — cosoh sina = u cosa + u sina 

w 00 ^oo * y 

where r may be negative. Substituting r in (Eq 4.12) by (Eq 4.15), 

n 

S = X ( (■*,-“ 4^) cosa+ G^-p ) sina) 2 . 
i = 1 



(Eq 4.14) 



(Eq 4.15) 



(Eq4.16) 
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Finally, 



^ = 2 ]T ( ( x . - \ l x ) cos a + (y • - ji y ) sina) (- U- - p^) sina + (y. - p^) cosaj 



/'= l 



/2 



2 X (O’.-F^) 2 - U/-H,) 2 ) sinacosa + 



n 



2 £ (.v, - p^) (y ; - n y ) (cos 2 a - sin 2 a) 



(Eq 4.17) 



( w 02 -jW 2o) sin 2a + 2m n cos 2 a 
0 . 



Therefore 



arctan2 (~2M 1V M 02 - M 2Q ) 



(Eq 4.18) 



a 



The solutions for the line parameters generated by a least squares fit are given by (Eq 4. 15) 
and (Eq 4. 1 8). 

The equivalent ellipse of inertia for the original n points is an ellipse which has the 
same moments around the center of gravity. M ma j or and M m i nor are moments about the 
major and minor axes respectively (Figure 4-3), 



M 



major 




(Eq 4.19) 



M • 

minor 




(Eq 4.20) 
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Origin 



X 



Figure 4-3. The Equivalent Ellipse of Inertia. De- 
termined for the set of points R , represented by the 
line L. 



The diameters d ma j or on the major axis and d minor on the minor axis of the equivalent el- 
lipse are 

d mmor = 4 -/'V, (Eq4.21) 

d ,najo, = 4 7", 00 . © 1 4 ' 22 > 

We define p, the ellipse thinness ratio, to be the ratio of d m [ nor and d ma j or: 



P = 



minor 



d 



major 



(Eq 4.23) 



A small p means a thin ellipse; as p increases toward 1, the ellipse degrades to a circle rep- 
resenting a thick line or “blob” of points. We will use p as an additional measure of the 
linearity of a set of points. 

The residual of a point p,- = (jc ,• yj) is 



5 = (H Jt -*,-)cosa+ (p^-y,) sin a . 



(Eq 4.24) 



Therefore, the projection, p’ t , of the point p i onto the major axis is 

P' i ~ Or, •+ 5 ; cosa, y ; + 5 ; sina) . (Eq4.25) 

We will use p\ and p' n as estimates of the endpoints of the line segment L obtained from 
the point set R by the least squares algorithm. In a sequential fitting process such as that 
employed on the AUV, only the first point, p x , need be stored during processing. Not only 
is the equation of the line important, but the estimation of both endpoints is also valuable 
information for sensor based navigation. 

B. BEGINNING A NEW LINE SEGMENT 

While the least squares fit method provides an appropriate estimate for a line passing 
through a set of points, it does not provide a method of determining when to terminate one 
line segment and when to begin another; which is the key to this method. This decision is 
critical for determining the best linearity fit for a set of range data. For example, if we 
receive a set of points representing two perpendicular surfaces, the least squares fit method 
will generate a single line that accommodates all of the points. This fails to provide an 
accurate depiction of the surfaces’ inherent linear features. We consider a set of range data 
points representing a corner of the swimming pool. This set of data with an unterminated 
line segment fit to it is seen in Figure 4-4. A line segment terminated with the described 
method is seen in Figure 4-5. 

1. Test for Residuals 

To determine when to end a line segment we perform two tests on the current line. 
The first test checks the goodness of the linearity fit for the most recent point. 

(x i+v >’ l + 1 ) . If the point satisfies 

8 { . + j < max{c 1 x o,c2) (Eq 4.26) 

where cl and c2 are positive constants (typically, cl = 3.0 and c2 = 0.4 ft.) and the standard 
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deviation, o, is 



° -2) (E<)1-27) 

then the point can be included in the current line segment. 




Figure 4-4. Unterminated Line Fit. Sonar returns from a corner 
of the swimming pool with an unterminated line fit to the points. 
Axis units in ft. 



2. Test for Ellipse Thinness 

The second test uses the ellipse information discussed above; if the thinness ratio 
for the line is smaller than the third constant, c3, then the set of points is still acceptably 
thin. We have modified the method of testing the thinness ratio from that used in [Ref. 15] 
by scaling the thinness criteria, c3 , based on the length of the line. If c3 is allowed to remain 
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Figure 4-5. Terminated Line Segment. Least squares fit line seg- 
ments terminated by applying tests described in text. Here, cl = 
3.0, c2 = 0.4, c3 = 0.1 initially. Axis units in ft. 



constant, then the ellipse thinness remains constant; however, as the line grows longer, this 
means that the acceptable width of the ellipse also grows. Using another actual data set, we 
can see in Figure 4-6 that the line is skewed slightly with respect to the predominant linear 
feature. This is due to the points representing the swimmer “pulling’' the line down because 
they still fell within the thinness ratio. For larger scale features, this means that a change in 
a feature will not be recognized until it has become large enough to fall outside of the thin- 
ness criteria. As an example, a line segment that is 1 00 feet long, developed using an ellipse 
thinness ratio of 0.1, will have a possible width of 10 feet. This potential width determines 
the necessary minimum size for another feature to force a termination of the line segment 
based on a constant thinness ratio. 
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To preclude this problem, we scale the thinness ratio as a function of length. The 
starting thinness ratio is determined as the desired accuracy of the smallest feature in the 
environment, with two feet being our chosen minimum usable size. Due to the physical 
dimensions of the swimming pool, the maximum length is set at 120 feet. At the small end 
of the spectrum, we have fixed the thinness ratio at 0.1, or 2.4 inches for the smallest 
features, while at the other extreme, a thickness of 1.2 feet is acceptable, which requires a 
ratio of 0.01. At intermediate lengths, the thinness ratio is determined by 



P 



0.1 - (0.09 



( 120.0 -length) 
1311.11 



) 



0.1015 



length 

1311.11 



(Eq 4.28) 



With this adaptive ratio, the feature in Figure 4-7 is depicted in a much more realistic fash- 
ion, with the line segments falling close to the actual walls, and the swimmer being 
outlined. 

If any point fails either of the two tests described, it is placed into a buffer which is 
used to initiate the next line segment. This method reduces the deleterious effects of noise, 
while maintaining the history of a possible feature change worth noting. When a line 
segment is terminated, the length is checked so that only those segments longer than a 
specified minimum (typically 2 ft.) will be processed by the pattern matcher. 



C. DESCRIPTION OF FEATURES IN TERMS OF AUV WORLD MODEL 



1. The Environmental Model 

Before obstacle recognition and positional updating can take place, a suitable 
environmental model must be defined. This environment model must facilitate the three 
functions of path planning, positional identification, and model updating. All three of these 
functions require the expression of the environment in some type of numerical form. 
Therefore, linear features are defined by a Cartesian coordinate system where some 
predefined point serves as the origin (see Figure 4- 1 for a depiction of the pool’s coordinate 
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system). In this manner, all features may be expressed in terms of their x-, y-, and z- 
coordinates where the linking of three or more vertices defines a polyhedron. 




Figure 4-6. Constant Ellipse Thinness Ratio. A constant value 
(c3) allows the actual width of the ellipse to grow as the line 
lengthens, producing a poor lit to the linear feature. Here cl = 
3.0, c2 = 0.3, c3 = 0.1. Axis units in feet. 



The structure used to link the points of a polyhedron is a linked list. This data 
structure is allocated in memory containing the x-, y-, and z-coordinates of each point. 
Pointers are then used to specify which vertices are connected to form the surfaces of a 
polyhedron. Additionally, the line parameters [r, aj defining each surface are stored in the 
list, with r being the positive distance from the origin to the line, and a being measured in 
a clockwise fashion similar to the AUV’s heading \\i. 
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Figure 4-7. Adaptive Ellipse Thinness Ratio. An adaptive ratio 
(Eq 4.28) maintains a more linear fit to the data points than using 
a constant ratio such as in Figure 5, cl = 3.0 and c2 = 0.3. Axis 
units in ft. 



2. Position Identification and Updating 

By using the parametric representation of a line (/, a), the process of matching is a 
simple matter of comparing the generated parameters with the parameters stored in the 
environmental model. The match criteria for r and a are designed to account for all known 
errors in generating the line segment. There are three primary errors: (1) positional 
uncertainty or navigational error, (2) inaccuracies due to the assumption that all echoes are 
along the sonar’s centerline (cosine effect), and (3) inherent sonar range error. We currently 
use an error of 3 ft. in r, and 0.34 radians (20°) in a. 
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If a generated line segment matches a known model feature with or without an error, 
then it is possible to identify the AUV’s position more precisely and to update the position 
based on the actual range to the feature, if an error exists. Utilizing the range to the feature, 
the known geometry of the sonar beam, and the angle of the sonar relative to the feature, 
an accurate normal distance can be computed. A single sonar is normally capable of 
updating the AUV’s position in one dimension at any given time. In the case of the left 
sonar, with the recognized feature parallel to the x-axis, the AUV’s position along the 
global y-axis would be determined as follows, 

>’AUV = feature- ran 8eX Sin (< V- fealure . > . (Eq 4.29) 

If the sonar is in contact with a feature that is parallel to the y-axis, then the AUV’s along 
the x-axis would be determined as follows, 

X AUV = r fca,ure~ ran 8e X COS (CL fealure ). (Eq4.30) 

3. Model Updating 

If no match is found in the environmental model, then we assume that the line 
segment was a result of a previously unknown feature, or obstacle. The parameters for this 
feature, developed as a planar surface, are stored as a new feature in the environmental 
model. This information may be needed for the path planning module, particularly if the 
new feature represents an obstacle to the currently planned path. Any new features that are 
added to the model will also be displayed graphically during post-mission data analysis. 
The line segment parameters (r, a) are stored with the feature for future pattern matching: 
hence, a previously unknown obstacle becomes a known environmental feature. 

The information generated by the least squares method is in two dimensions only, 
while the AUV is operating in three dimensions. For the forward and side sonars, the two 
dimensions used are the global x and y coordinates, while for the bottom sonar the z 
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information is treated as ‘y ’ data for use with the algorithm. Features seen by the side sonars 
are in the vertical plane, while those seen by the bottom sonar are in the horizontal plane. 

Figure 4-8 shows the acoustic signal pattern of each transducer as a spherical 
section. A valid range data point could be produced by any reflecting surface patches of an 
object on this spherical surface. We make the assumption that the range return was from 
the beam’s centerline. While it is possible to correct this range once the orientation of the 
feature is known, we do not want to preclude the possibility that a previously unknown 
obstacle of unknown orientation may be producing the return. The errors induced in making 
this assumption will be absorbed by the least squares algorithm to some extent, with the 
remaining error being accounted for in the matching process. 

If a line segment is not successfully matched to a known surface in the 
environmental model, then we must process the segment for further use. Once the 
endpoints are determined, a surface with length equal to the length of the line segment, and 
width proportional to the range from the AUV to the endpoint at the time it was obtained 
(Figure 4-9), is generated. The width is determined as 

d = rsin (5°) = rx 0.087. (Eq4.3l) 

At the selected maximum range of 30 meters (98.25 ft.), d = 2.6 m, (7.86 ft.). Choosing 
this width accounts for the fact that the return could have come from any point on the sur- 
face of the spherical section created by the signal. Thus, a 3-D surface is produced from 2- 
D information. The generated polygon is stored as a set of nodes in the environmental mod- 
el for used in future path planning and post-mission data visualization. At this time, we do 
not provide for the retraction or resizing of surfaces based on subsequently obtained infor- 
mation. 
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Figure 4-8. Cross-sectional View of a Sonar’s Beam Pattern. The dis- 
tance d and the error e are proportional to the range. The curvature 
of the spherical surface is exaggerated for illustration purposes. 




Figure 4-9. Projection of Corner Points for 3-D Surface. Corner points 
generated from the line L. The distance d is determined by (Eq 4.31). 
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V. SOFTWARE PROCESSES FOR REAL- 
TIME OBSTACLE AVOIDANCE 



The sonar system installed in the AUV II vehicle plays an integral role in the execution 
of planned missions. The system interfaces with several other processes, as illustrated by 
the current working version of the dataflow diagrams for the AUV software (Figure 5-1 and 
Figure 2-3). The sections below describe the various sonar software processes and the 
interfaces to the other processes seen in Figure 5-1. The interface with the environmental 
database was discussed in Section C of Chapter IV. The actual code associated with the 
sonar processes is found in Appendix B. 

A. SONAR SOFTWARE PROCESSES 

The process READ SONAR DATA (5.1) in Figure 5-1 obtains the current range value 
for the appropriate sonar by reading the proper channel of the GESPAC-2b DA/AD 
converter. This raw data is then used to update the moving average in the FILTER DATA 
process (5.2), as described in Section A. 6 of Chapter III. The obstacle_alert flag will also 
be set here, as described in the next section. The new average range is then used by the 
UPDATE LINE SEGMENT (5.3) process to develop a line segment. If the point is valid 
for the current line segment, then the line parameters are updated and compared to the 
known obstacles by the process MATCH LINE TO MODEL (5.4). If no match is found, 
then the new _obstacle flag is set. 

If the new data point causes a termination of the current line segment based on the tests 
described in Section B of Chapter IV, and the new_obstacle flag is set, then a new obstacle 
is developed and added to the environmental database as described in Section C.3 of 
Chapter IV by the ADD NEW OBSTACLE process (5.7). If the line segment is terminated, 
then all variables are cleared and a new line will start with the next incoming range data 
point. Once the match process (5.4) is complete, a position update (see Section C.2 of 
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Figure 5-1. Sonar Process Dataflow Diagram. 



Chapter IV) can be performed if the obstacle was found in the database, i.e. the new _obsta^ 
cle flag is not set. 

B. INTERFACE WITH CONTROL GUIDANCE SYSTEM 

There are two occasions when information obtained by the sonar might dictate motion 
control for the AUV. The first instance is the most critical, it is simple obstacle avoidance. 
The second instance arises when it becomes desirable or necessary to map the extent of a 
previously unknown obstacle; this case is discussed in the next section. Sonar range 
information that indicates a potential collision in any direction will be recognized by the 
FILTER DATA process (5.2) in Figure 5-1. 

A minimum safety range for each direction from the AUV (forward, left, right, down) 
is stored as part of the sonar range data structure. As the latest range reading is filtered, the 
minimum safety range is compared to the average actual range for that direction. If any of 
the actual ranges fall below the minimum safety range, the obstacle _alert flag bit is set for 
that sonar/direction. This flag is passed to the AVOID OBSTACLES process (8) in Figure 
5-1. 

The AVOID OBSTACLES process determines which course of action to follow if any 
of the flag bits are set. A heading change, a depth change, a full stop, or any combination of 
these changes may be necessary. The various options are shown in Table 5-1. The currently 
installed software generates an “emergency posture” that forces a turn or depth change to 
avoid areas flagged as containing threatening obstacles. While operating in the NPS swim- 
ming pool environment, all turns are ninety degrees. Since the AUV’s dynamics will carry 
it closer to an obstacle during the turn, a logical lockout system prevents the sonar ranges 
from generating any more alerts until the AUV is within 20° of the new heading. If there 
are no known lateral obstacles, an obstacle ahead of the AUV could be avoided by a turn 
to the left or to the right. The default in this case is a right turn. 
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TABLE 5-1. AUV OBSTACLE AVOIDANCE MANEUVERS 



Obstacle Alert Flag 

(Fwd, right, left, bottom) 


Turn 


Depth Change 


OXXO 


— 


... 


0XX1 


... 


ascend 


1101 


left 


ascend 


1100 


left 


... 


1011 


right 


ascend 


1010 


right 


... 


1 1 IX 


stop 


(ascend) 


0 = no obstacle 


1 = obstacle 


X = 0 or 1 



C. MOTION ALGORITHM FOR OBSTACLE MAPPING 

The most reliable method of mapping an underwater feature is to control the motion 
of the vehicle so that the range to the feature remains within the optimum range of the 
sensor, but does not present a collision danger. As described in Chapter IV, the linear 
feature extraction method provides the best resolution at short ranges; therefore, the 
minimum safety range becomes the determining factor in choosing a mapping distance. 

Mapping of the bottom topography with the down-looking sonar is much simpler than 
maintaining a fixed distance laterally from a feature. As noted earlier, the heading of the 
AUV has no effect on the feature extraction of targets below the AUV. Due to the AUV’s 
inherent stability, pitch and roll will affect the bottom sensor only in extreme cases. A 
simple terrain following altitude controller has been implemented and tested in the NPS 
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swimming pool. The control law used was 

dels = k range ( ran S^ C om ~ ran 8 e ) + k thetcP + M ^ 5 - ! ) 

where dels is the commanded dive plane angle, range com is the desired altitude above the 
bottom, range is the actual sonar range to the bottom, 0 is the pitch angle, and q is the pitch 
rate. 

Mapping of lateral features with the left or right sonars is complicated by the need to 
control heading while attempting to maintain mapping range to the feature. For this reason, 
a simpler scheme involves generating intermediate waypoints and allowing the guidance 
and control processes to maneuver the AUV. Care must be taken to ensure that a large 
lateral step is broken down into smaller steps to ensure that the sonar in contact can 
maintain contact during the maneuver. This means heading changes of no more than five 
degrees relative to the surface of the feature to obtain the highest resolution. The desired 
feature resolution and mission task will ultimately dictate the magnitude of these turns. 

D. INTERFACE WITH ONBOARD MISSION REPLANNER 

At any time that an obstacle is detected, either as a collision danger, or as a previously 
unknown feature, the new _obstac!e flag is set. This flag, in conjunction with the 
obstacle_alert flag, is used to signal the mission executor. One of the roles of the mission 
executor is to determine from these flags if path replanning is necessary, or if obstacle 
mapping may also be required. In order for the mission replanner to accurately assess the 
path to the goal, it must have a complete environmental database. If an emergency turn was 
made due to an obstacle ahead of the AUV, it may be necessary to generate obstacle 
mapping waypoints to determine the extent of the obstacle. This area of research is 
addressed by Wilkinson [Ref. 24], and is a matter of ongoing research. 
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VI. EXPERIMENTAL RESULTS 



A. EXPERIMENTAL RESULTS OF MODEL MATCHING 

Satisfactory tests of the model matching algorithm discussed in Chapter IV were 
conducted with the AUV operating in the swimming pool. The range data points shown in 
Figure 6-1 are from one such test. Again, the errors previously discussed are manifested in 
an imperfect match to the pool walls. However, by accounting for these known errors in the 
matching, matches between generated line segments and the environmental model are 
possible. 




Figure 6-1. Sample Range Data Set. Range data set used for by the model 
matching algorithm, taken during a test in the NPS swimming pool. Bold 
rectangle represents pool walls. 



43 



Using an error of 3 ft. in r, and 0.34 radians (20 degrees) in a, the generated line 
segments in Figure 6-2 represented as dashed lines matched features in the model correctly, 
with no false matches generated. 




Figure 6-2. Model Matching Results. Line segments fit to the data set 
shown in Figure 6-1, with those segments that were identified by the model 
matching algorithm shown with dashed lines. Numbers represent element 
of model matched, with “s9s” representing no match. Bold rectangle repre- 
sents pool walls. 



B. REAL-TIME OBSTACLE AVOIDANCE 

The software processes used to perform obstacle avoidance were discussed in Chapter 
V. The results of a pool test mission are seen in Figure 6-3. The original preplanned 
waypoints for the mission are represented by an “x”, the point corresponding to the 
minimum safety range is marked with an “O”, and the obstacle avoidance waypoint 
generated by the AVOID OBSTACLES process is shown with a “W”. As demonstrated by 
this test, the sonar processes and gross motion control discussed in Chapter V work as 
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designed, with the AUV safely avoiding an obstacle directly ahead and within the minimum 
safety range. 

C. ALTITUDE CONTROL 

The terrain-following altitude controller discussed in Section C of Chapter V was tested 
in the NPS pool. Using a filtered signal from the bottom sonar, the controller maintained the 
AUV at a height of 3.0 feet from the pool bottom. As illustrated in Figure 6-4, the controller 
maintained altitude after the initial dive with an error not exceeding 0.5 ft. The tests, using 

the control law from (Eq 5.1), were conducted with k ranRe = 1.1 and k theta = 3.5 , and 
k q = 2.5. 
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Figure 6-3. Obstacle Avoidance Test. AUV track is shown in upper box, forward so- 
nar range is graphed below’. Obstacle within 28 ft. was detected at points marked 
with “O”, resulting in new waypoints (“YV”) to replace original track (“x”). 



46 





0 

4 

8 



Figure 6-4. Altitude Controller Performance. AUV depth was commanded to be 3 
ft. and was controlled using the bottom sonar. Pool depth profile during mission is 
shown in lower graph. 
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VII. SIMULATION AND POST-MISSION 3-D DATA MAPPING 



A. AUV MISSION SIMULATOR 

1. Overview 

The role of simulation in a project such as the AUV II project is very beneficial, 
both for testing new processes and for analyzing actual data collected during a test mission. 
Some of the benefits of the current simulator include: 

• Allows software developers to test and debug new modules without the effort 
of a full field test 

• Provides visual display of sonar returns 

• Provides the ability to develop and observe AUV missions before they are 
actually run 

• Provides the ability to visualize the results of a mission from the actual data 
collected during a test. 



The AUV mission simulator is based on the simulator developed by Jurewicz and 
detailed in [Ref. 25]. This simulator, running on a Silicon Graphics IRIS workstation, 
provides a 3-D graphical presentation of the NPS AUV II as it moves through the NPS 
swimming pool, or the Monterey Bay. The simulated AUV is controlled via either mouse 
input through the interface screen, or by manipulating the 6-degree-of-freedom Spaceball 
input device. Velocities, accelerations, and position changes are calculated using the 
control inputs for motor speeds and fin deflections. A detailed hydrodynamic model of the 
AUV is used to compute the simulated motion of the vehicle. 

2. Sonar Simulation 

In order to fully simulate the AUV in the pool environment, a simulation of the 
sonar signals was developed using graphics ray tracing techniques. Each sonar transducer 
is modeled as a point source for a sound ray, with a single ray emanating from the source 
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in the direction of orientation of the sonar. Since the sonars actually emit acoustic energy 
in a cone pattern, six additional rays are simulated to form the outer surfaces of the cone, 
spreading out from the source at a five degree angle (see Figure 7-1). 




Rays. Seventh ray is in center, along sonar axis. 



Each ray is traced in a recursive fashion as it intersects the polygons forming the 
pool boundaries. Each point of intersection is stored in an array, and the angle of reflection 
is calculated. Each point then becomes a new source, and the next level of reflection points 
is calculated. This recursion continues until a level of four reflections is reached. If the total 
distance travelled in reaching a point exceeds two times the maximum range of the sonar 
(30 m), then that ray is terminated regardless of the level of recursion, since it would arrive 
back at the sonar after the next ping started. The ray-tracing code is adapted from an 
algorithm found in Glassner |Ref. 26]. 
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After all of the points of intersection are determined and stored, the point found to 
be closest to the sonar, with a reflecting angle within the sonar’s field of view, is 
determined. For a point to be closest, the distance that the ray has travelled to that point 
added to the Euclidean distance from the point to the sonar, must be smaller than for any 
other point. This distance is then divided by two, and a point is projected from the sonar in 
the direction of its orientation, since we assume that all sonar returns come from points 
along the sonar beam’s axis. Any points that were reached by one or more reflections will 
appear farther from the sonar than the actual object (see Figure 7-2). This becomes most 
pronounced when the sonar is directed toward the vertex of a concave corner. 




Figure 7-2. False Apparent Range Caused by Re- 
flection. 



B. POST-MISSION REPLAY 

All of the actual test missions run in the NPS pool provide a mission data file that is 
downloaded from the AUV at the end of each test run. This information is utilized for 
parameter analysis and sonar performance evaluation. The replay capability of the original 
AUV simulator has also been expanded to accept the data files from actual missions. The 
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data file is loaded onto the graphics workstation where mission replay is selected. Each 
control parameter is used to position the control surfaces, and the positional information is 
used to determine the AUV’s position and orientation for display. 

Additionally, the range values for each sonar are projected from the appropriate sonar 
location for display. As the AUV moves through the mission replay, the track is displayed, 
as well as the actual sonar returns. This capability allows the users to determine if the pre- 
mission simulation was accurate, or if the AUV may have reacted to a situation not foreseen 
by the testers, as evidenced by an unexpected sonar return or track change. The completed 
replay of an actual AUV pool mission is seen in Figure 7-3, with the actual recorded track 
and sonar information labelled. 

The 3-D surfaces discussed in Chapter IV representing unknown obstacles can be 
displayed as well. This capability would have much more significance for a vehicle 
operating in open waters, or in an area where the environmental database was sparse. This 
capability, although limited by the resolution and number of sensors, may well support 
many major missions for AUVs. 
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Figure 7-3. Graphical Mission Replay. View of simulated pool from above follow- 
ing replay of mission sonar and track data. Apparent errors are due to DR naviga- 
tion inaccuracies. 
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VIII. CONCLUSIONS AND RECOMMENDATIONS 



A. CONCLUSIONS 

This work addressed four primary objectives, as reiterated here: 

• How can data from ultrasonic sensors be best utilized to recognize obstacles 
during operation of the NPS AUV II? 

• What is the optimal configuration for ultrasonic sensors on the AUV II to 
provide obstacle detection and terrain data collection for post-mission 
analysis? 

• What type of motion algorithm will best provide collision avoidance and 
obstacle feature extraction? 

• How can sensor data be utilized in post-mission analysis to generate a 3-D 
terrain model? 

After conducting the development, testing, and evaluation of the various processes 
described in this work, we have reached the following conclusions in response to the 
questions posed in the objectives: 

• The linear feature extraction method presented here, having been initially 
developed on a land vehicle in two dimensions [Ref. 15], proved itself robust 
enough to provide useful information for an AUV operating in three 
dimensions, as well. 

• Given the current availability of four sonar systems, the orthogonal mounting 
arrangement discussed in Chapter II proved satisfactory for obstacle 
avoidance, altitude control, and feature extraction. 

• In the swimming pool environment, the processes discussed for obstacle 
avoidance and terrain data collection were demonstrated to be satisfactory. 

• The extension of an existing graphical simulation to include a mission replay 
capability provided a satisfactory tool for analysis of 3-D mission data. 

A key factor in the successful development of this work was the availability of a 
readily adaptable graphical simulator. The effort involved in conducting pool tests with the 
AUV, and competing research requirements, precludes frequent testing of individual 
aspects of the vehicle’s systems. Access to a dynamic simulator ensured that basic 
algorithm development and testing could be done independent of the actual vehicle. 
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The goal of classifying obstacles is currently being pursued by others involved in the 
NPS AUV research project. Due to the inherent noise in the sensors and the low resolution 
of the sonars as currently installed, classification of objects must be limited to those objects 
in excess of two feet in size. The calculation of position based on inertial sensing is 
currently being developed and should provide more reliable results in the future, as well. 

B. RECOMMENDATIONS 

The capabilities of NPS AUV II continue to grow as new processes and systems are 
developed and installed. In order to enhance the obstacle avoidance capabilities of the 
vehicle, the following specific recommendations are provided for future work: 

• Provide for the installation of more sonar transducers to provide increased 
coverage of vehicle surroundings. 

• Eliminate the on-board averaging performed by the current sonar processing 
boards. 

• Install an accurate inertial system to provide more accurate positioning data. 

• Consider providing additional processing capabilities to ensure that the 
required control system sampling rates are maintained. 

• Develop simulation capabilities ahead of any new functionality that is installed 
in the vehicle. 

The NPS AUV II project should continue to be a viable research effort for many years, 
and these recommendations are made to ensure that future efforts will be able to build on 
this and other current work. 
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APPENDIX A 



AUV DATA DICTIONARY 



Baseline System (vl.l) 

Symbols for Describing Data 
NAME = Description 

Description + Description... (denotes aggregate structure “+”) 

mm {Description} max (denotes multiple instances “{ }”; if min not specified, assume 

empty; if max not specified, assume infinite) 
description I Description> (denotes mandatory “<>” choice “I”) 

(Description) (denotes optional item “()”) 

Predefined Data Names 

CHAR = Letters, numbers, space, punctuation 

STRING =0{CHAR}239 

EMPTY = absence of value 

DIGIT = 0.. 9 

DAY = 1{ DIG IT} 2 

MONTH =1{DIGIT}2 

YEAR = 4 {DIGIT} 4 

DATE = DAY + MONTH + YEAR 

INTEGER =1 {DIGIT} 

REAL = {DIGIT} + . + {DIGIT} 

SECOND=2 { DIGIT } 2 

MINUTE = 2{DIGIT} 2 

HOUR = 2 {DIGIT} 2 

TIME = HOUR + MINUTE + SECOND 
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NOTE: BOLD TYPE NAMES REFER TO current Baseline System DFD 



Point_3D = 
x = 


REAL + 


y = 

z = 


REAL+ 

REAL 



Linear_velocity_3D= 
u = REAL + 

v = REAL+ 

w = REAL 

Linear_accel_3D= 

u_dot= REAL + 



v_dot= 

w_dot= 


REAL + 
REAL 


Attitude_3D= 
phi = 
theta= 
psi = 


REAL + 
REAL + 
REAL 



Rotational_velocity_3D= 
p = REAL + 

q = REAL + 

r = REAL 

Rotational_accel_3D= 
p_dot= REAL + 



q_dot= 

r_dot= 


REAL + 
REAL 


Fin_angle = 


REAL 


Polygon = 
Posture - 
Position 


3{Point_3D) 

= Point_3D + 



Velocity = Linear_velocity_3D + 

Acceleration= Linear_accel_3D + 
Attitude = Attitude_3D + 
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Rotation_vel= Rotational_velocity_3D + 
Rotation_acc= Rotational. accel_3D 
Time = TIME 

Mission_requirement= 

Start_point = Point_3D + TIME + 

Intennediate_point={Point_3D + TIME) + 
Goal = Point_3D + TIME 

Mission_type = INTEGER 

Path = 2 {Posture) 

Referenceposture =Posture 

Current_posture= Posture 

Rcplan_request= BOOLEAN 

Range_data= 

Range= REAL + 

Error= REAL 

Sonar_data= Range_data + 
Transducer_no=INTEGER 

Track_data= Current_posture +4{Sonar_data}4 

Commanded_posture=Posture + 

Mode = INTEGER 

Emergencv_posture=Posture 

Status = BOOLEAN 

System_status=n { BOOLEAN ) n 
Note: n is number of systems reporting 

ObstacIe_aIert=BOOLEAN 

Kno\vn_obstacles= {Polygon} 

New_obstacIe = 1 { Polygon } 1 
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Position_update = Point3 + TIME + 

Confidence_factor =INTEGER 

Inertial_data = Rotational_velocity_3D + Attitude_3D + 

Depth = REAL + 

Fwd_speed= REAL + 

Mag_hdg= REAL 

Controlsignal = 

Fin_deflection=8{Fin_angle}8 + 

Rt_main_rpm= { DIGIT } 3 + 

Lt_main_rpm={DIGIT}3 + 

Fwd_hor_rpm={ DIGIT } 3 + 

Aft_hor_rpm= {DIGIT} 3 + 

Fwd_ver_rpm={DlGlT}3 + 

Aft_ver_rpm= {DIGIT} 3 

Control_positions = Control_signal 
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APPENDIX B 



AUV CODE 



AUV.H 

Main header file for (he auv control code. 

**************************************************************************** *********y 
#include <sidio.h> 



#ifndef EXTERN 
#define EXTERN extern 
^define INIT(x) 

#endif 



#ifndcf MAIN 
#dcfine EXT extern 
flendif 

flifdef MAIN 
# define EXT 
#undcf MAIN 
#endif 



#define DAC2B_ADDR 0xFFF00040 /* GESDAC-2b addr */ 

#define DAC_LSB_OFFSET 0x2 
#dcfinc DAC1_ADDR OxFFFOOOOO 



#define ADC1_ADDR (DAC1_ADDR + 0x11) 
#define ADCl_MSB0x0 
#define ADC1_LSB 0x2 
#define ADC1_CMD_REG 0x4 
#define ADC 1_STATUS_REG 0x4 
#definc ADC1_BUSY 0x40 

Adeline ADC2_ADDR 0xFFF00020 
Adeline ADC2_CH_GAIN 0x0 
A define ADC2_STATUS_REG 0x2 
#definc ADC2_DATA 0x1 
Adeline ADC2_CMD_REG 0x2 

Adeline MF1_BASE 0xFFF00700 

#definc VIA0_ADDR 0xFFF00080 
#define ORBJRB 0 
#define ORAJRA 1 
#dcfinc DDRB 2 
#define DDRA 3 

#definc T1C_L 4 
#definc T1C_H 5 



/* GESPIA-3A bus addr */ 
/* GESPIA-3A i/o reg B */ 
/* GESPIA-3A i/o reg A */ 
/* Data direction reg */ 

/* Data direction reg */ 
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#define T1L_L 6 
^define T1L_H 7 
#define T2C_L 8 
#define T2C_H 9 
#define ACR 1 1 
# define PCR 12 
#define 1FR 13 

^define FRONT_RUD_TOP 0 
^define FRONT_RUD_BOT 1 
#define FRONT_PL_R IG HT 2 
#dcfine FRONT_PL_LEFT 3 
#define REAR_RUD_TOP 4 
#define REAR_RUD_BOT 5 
#define REAR_PL_R1GHT 6 
#define REAR PL_LEFT 7 



#define RIGHT.MOTOR 0 
#define LEFT_MOTOR 2 
#define SUPPLY 1 

#define RIGHT_MOTOR_RPM 0 
#define LEFT_MOTOR_RPM 1 
#define ROLL_ANGLE_CH 7 
#define PITCH_ANGLE_CH 6 
#define ROLL_RATE_CH 9 
#define PITCH_RATE_CH 8 
#define YAW RATE CH 10 



/* port register A / data direction A */ 
/* control register A */ 



/* control register B */ 



struct MFI_PIA { 
unsigned short pra; 
unsigned short era; 
unsigned short prb; 
unsigned short erb; 



EXT unsigned char Read_PortA(),Read_PortB(); 
EXT unsigned short Read_PorlAB(); 



EXT FILE *outfpl; 

EXT FILE *outfp2; 

EXT int datajength; 

EXT int loopent; 

EXT int end_test; 

EXT int wrap_count; 

EXT double t; 

EXT double rpm; 

EXT double main_motor_deltal,main_motor_delta2; 

EXT int main_motor_voltl ; 

EXT int main_motor_voll2; 

Function declarations 

****♦***************♦**♦**♦************♦+****+****** j 

EXT void usc^interfaceQ, control_module(); 
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EXT void loop_data(), zero_data(), control_surface(), record_data_on(); 

EXT void record_data_off(), record_data(); 

EXT void initialize_dacsOJnitialize_adcs(); 

EXT void rudder(),planes(); 

EXT void main_motors off(),alivcO: 

EXT void dacl(),dac2b(); 

EXTintadclO,adc2(); 

EXT double heading(),pitch_angle(),roll_angle(),calc_psi(); 

EXT double rolLrate_gyroO,yaw_rate_gyro(),pitch_rate_gyro(); 

EXT double depthO; 

EXT double right_m_rpm()defl_in^m(); 

EXT double get_speed(); 

EXT unsigned short psi_bit_old; 

EXT double old_angle; 

EXT double dg_offsetJc_psi,k_r; 

EXT double k_z,k_theta,k_q; 

EXT double k_speed; 

EXT double ki_specd; 

EXT double timejimit; 

EXT int num_break; 

EXT unsigned start_dwell; 

EXT int direction; 

EXT double x_com[20],y_com[20],z_com[20]; 

EXT double psi_com_offset; 

EXT double psi_com_set; 

EXT double psi_com; 

EXT double delta_x_y_z; 

EXT double x, x_init; 

EXT double y, y_init; /* inii pos input in userJnterfaceQ */ 

EXT double speed; 

EXT int roll_rate_0,pitch_rate_0,yaw_rate_0; 

EXT int roll_0,pitch_0; 

EXT int z_valO; 

EXT int pointer; 

EXT int speed_array [ 11]; 

EXT double speed_limit, delta_speed; 

EXT double delta_sum_spced; 

EXT int tick, tick 1 , tick2, curr_tick, mask, count; 

EXT double value; 

EXT long date,time; 

EXT short day; 

EXT char ans; 

^****************************************** *********** 

Sonar related data items/functions/structures 

**************************************************** j 

tfdefine NEWTYPE(x) (x *)(malloc((unsigned)sizeof(x))) 

j ***************************************************** 

Sonar switch addresses for use 
with GESPIA interface 

**************************************************** j 

#dcfme SONAR_SWl OxOE 
#dcfinc SONAR_SW2 OxOD 
#dcfinc SONAR_SW3 OxOB 
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Adcfinc S0NAR_SW4 0x07 
Adefine S0NAR_TRIG1 0x10 
Adefine S0NAR_TR1G2 0x20 

Constant values for use with 
sonar line fitting and obstacle 
avoidance 

jjc******^***************^*^*^********^*********^****^ f 

Adcfinc CON VERT_TO_FEET 0.02398 
Adeline MlN_OBSTACLE_RNG 25.0 
Adcfinc AVG.PTS 10 
A define MAX_RNG_D1FF 15.0 
Adefine MAX_RNG_D1FF_FWD 25.0 
Adcfinc MAXJPTS 1200 
Adefine DEG_TO_RAD 0.017453 
Adefine FALSE 0 
Adcfinc TRUE 1 

/* Constants for line seg package */ 

Adefine cl 3.0 
Adefine c2 .6 
Adcfinc c3 .20 
Adefine MlN_NO_PTS 3 
Adefine MAX_BAD_PTS 3 

Aifndcf SONAR 
Adefine EXT extern 
Aendif 

Aifdef SONAR 
Adcfinc EXT 
Aundcf SONAR 
Aendif 

/***************************************************** 

Variable declarations 

**************************************************** I 

EXT interror, range, sw_cnt, sw_com, bad_rng, bad_updates, rangejndex; 

EXT int b_range, bad_pt_no, obstaclc_alcrt, ncw_obstaclc; 

EXT double range 1; 

EXT double range2; 

EXT double range3; 

EXT double rangc4; 

EXT double error 1; 

EXT double crror2; 

EXT double avg_mg, range_com, k_range, max_delta_r, max_delta_theta; 

EXT double rangc_arrav[10]; 

EXT double bad_pt_buffer[MAX_BAD_PTS][2]; 

EXT double pool[6](2]; 

EXT double max_dclta_r, max_delta_lheta; /* input in user_interfacc() */ 

j * + *** + ^******* + *3fc*:f:****sfr****atc****sfrat<*+***sfr**5t<** !l k*afr*sfrsfr* 

Function declarations 

EXT void get_init_avg(), get_avg_mg(), initializc_sonars(); 

EXT void ping_sonars(), sonars_on(), set_up_sonars(); 

EXT double atan2(); /* not provided by math.h in current compiler */ 

EXT void init_pool(); 

EXT void match_model(); 

EXT void resct_line(); 



/* GESDAC2 units ->ft */ 

/* Obs avoidance mg (ft) */ 

/* filter avg window size */ 

/* filter bandpass sides/bottom*/ 
/* filter bandpass-fwd sonar */ 



/* sigma factor */ 

/* deviation const */ 

/* ellipse thinness ratio */ 

/* least sqrs min */ 

/* least sqrs bad pt buffer size*/ 
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EXT void record_line_on(); 

EXT void record _line_off(); 

EXT void line_segment_init(); 

EXT void computeJine_seg(); 

EXT void end Jine_segment(); 

EXT void swap_init_pointsO; 

EXT void convert_coords(); 

/***************************************************** 
Data structures 



typedef struct 

{ /* Data for each sonar’s line segment */ 

double sonar_data_pts[MAX_PTSJ[2]; /* t,rng */ 

double plane_pts[4][3]; 

double sgmxx, 

sgmyy, 

sgmxy, 

sgmyx, 

sgmx, 

sgmy, 
r_sonar, 
theta_sonar; 
double mux, 
muy, 

muxx, 
muyy, 

muxy, 
delta_x, 
delta_y, 
sgmx2, 
sgmy2; 

double sgm_delta_sq, 

sigma, 

d_major, 

d_minor, 

m_major, 

m_minor; 

double delta Jine, 

start_pt_x, 

start_pt_y, 

end_pt_x, 

end_pt_y; 

double linejength; 

int i_s, 

j_s, 

n_s, 

k_s, 

end_pt_no, 

start_pt_no, 

bad_pt, 

too_long, 

too_far_apart, 

course_change; 

nt depth_change, 

range_pt, 

loc, 

min_x, 

max_x; 

) LINE.SEGMENT f *LINE_SEG; 
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typedef struct 

( 

int range_index, 
bad_mg, 
bad_updatcs, 
switch, 
trigger, 
sonarid; 

doublerange_array[AVG_PTS], 

range_total, 

avg_mg, 

max_mg_diff, 

min_obs_mg, 

raw_mg; 

}RANGE_DATA; 

j** *************************************************************************** ******** 

AUVMAIN.C 



Main control code for the auv. 
#define MAIN 



#include <ermo.h> 

#include <setsys.h> 

#include <math.h> 

^include “auv.h” 

unsigned char*dacl_a = DAC1_ADDR; 
unsigned char*dac2b_a = DAC2B_ADDR; 
unsigned char*adcl_a = ADC1_ADDR; 
unsigned short*adc2_a = ADC2_ADDR; 
unsigned short*viaO = VIA0_ADDR; 



/* 4 Channels of DAC ADA-1 DAC */ 
/* 8 channels of DAC DAC-2B */ 

/* 16 channels of ADC ADA-1 */ 

/* 16 channels of ADC ADC-2 */ 

/* PI A addr */ 



void init_vars(); 
void init_clock(); 
void check_clock(); 

main() 

( 

RANGE_DATA *front_sonar,*left_sonar, 

* righ t_sonar,* bottom_sonar; 
LINE_SEGMENT *leftjine; 

count = 0; 
mask = OxOOOOffff; 
obstacle_alert = 0x0; 
new_obstacle = 0; 

front_sonar = NEWTYPE(RANGE_DATA); 
left_sonar = NEWTYPE(RANGE_DATA); 
right.sonar = NEWTYPE(RANGE_DATA); 
bottom_sonar = NEWTYPE(RANGE_DATA); 

leftjine = NEWTYPE(L1NE_SEGMENT); 



init_vars(); 
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loop_data(); /* Get z_com & x_com & y_com arrays */ 

userjnterface(); 

initialize__dacs(); 

iniiialize_adcs(); 

printf(“ Position AUV for Directional Gyro Offset Measurements”); 

printf(“ Rate Gyro zero measurements”); 

printf(“ Hit Any Key When ReadyS”); 

ans = getchar(); 

ans = getchar(); 

set_up_sonars(front_sonar, right_sonar, Ieft_sonai\ 

bottom_sonar); /*pass in all four sonars in this order*/ 

sonars_on(front_sonar, left_sonar); 

/* pass in two sonars, must be one from each board-> (front or 
right) followed by (left or bottom) */ 

zero_data(); 

printf(“StartingS”); 

psi_bit_o!d = Read_PortAB(MFI_BASE); 
psi_bit_o!d &= 0x3FFF; 

alive(10,start_dwell); /* Wag fin every 10 seconds for total 

duration of start_dwell seconds */ 

record_data_on(); 

record_line_on(); 

initialize_sonars(front_sonar, lcft_sonar); 

initialize Jinc(left_line, 3); 

while (end_test) /* 10Hz control loop */ 

( 

init_clock(); 

get_avg_mg(front_sonar); 

get_avg_mg(left_sonar); 

update_line_seg(left_line); 

control_module(front_sonar, right_sonar, left_sonar, bottom_sonar); 
ping_sonars(fronl_sonar, lcft_sonar); 
check_clock(); /* timed loop control */ 



main_m otors_off() ; 
record_data_off(); 
endjine_segment(left_line); 
record_line_off(); 



}/* end main() */ 

init_vars() 
void init_vars() 
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{ 

datajength = 0; 
loopcnt = 0; 
end_tesi = 1; 
wrap_count = 0; 
t = 0.0; 

main_motor_voltl = 512; 
main_motor_volt2 = 512; 
direction = 1; 
old_angle = 0.0; 
psi_com_offset = 0.0; 
x = 0.0; 
y = 0.0; 
speed = 0.0; 
pointer = 0; 

delta_sum_speed = 0.0; 

)/* end init_globals() */ 

y***************************************************** 

init_clock() 

**************************************************** j 

void init_clock() 

( 



_sysdate(3,&time,&datc,&day,&tick); 
tickl=(Lick & mask); 

}/* end init_clock() */ 

y***************************************************** 

check_clock() 

**************************************************** y 

void check_clock() 

{ 

_sysdate(3 t &time,&date,&day,&tick); 
tick2=( tick & mask ); 
if (tick2 < tickl) 
tick2 = tick2+100; 
value=abs(tick2-tick 1 ); 
while (value != 10) 

{ 

if (Lick2 < tickl) 
tick2=tick2+100; 
value=abs(tick2-tickl); 
_sysdate(3,&time,&date,&day,&tick); 
tick2=( tick & mask); 

} 

}/* end check_clock() */ 



y* ************************************************************************************ 

The following sonar code is found in the file asonar.c. 

AUV sonar modules called from main loop of control program or 
from other sonar modules. 

*************************************************************************************y 

#define SONAR 
#includc “auv.h” 
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/***************************************************** 

set_up_sonars() 

**************************************************** I 

void set_up_sonars(sonarl, sonar2, sonar3, sonar4) 

RANGE_DATA * sonarl, *sonar2, *sonar3, *sonar4 ; 

{ 

sonar l->bad_mg = 0; /* front */ 
sonar2->bad_rng = 0; /* right */ 
sonar3->bad_mg = 0; /* left */ 
sonar4->bad_mg = 0; /* bottom */ 

sonar l->bad_updates = 0; 
sonar2->bad_updates = 0; 
sonar3->bad_updates = 0; 
sonar4->bad_updates = 0; 

sonar l->switch = SONAR_SWl; 
sonar2->switch = SONAR_SW2; 
sonar3->switch = SONAR_SW3; 
sonar4->switch = SONAR_SW4; 

sonarl ->trigger = SONAR_TRIG 1 : 
sonar2->trigger = SONAR_TRIG 1 ; 
sonar3->trigger = SONAR_TRIG2; 
sonar4->trigger = SONAR_TRIG2; 

sonar 1 ->range_total - 0; 
sonar2->range_total = 0; 
sonar3->range_total = 0; 
sonar4->range_total = 0; 

sonarl ->max_mg_d iff = MAX_RNG_DIFF_FWD; 
sonar2->max_mg_diff = MAX_RNG_DIFF; 
sonar3->max_mg_diff = MAX_RNG_DIFF; 
sonar4->max_mg_diff = MAX_RNG_DIFF; 

sonar l->min_obs_mg = 28.0; /* values for example only */ 

sonar2->min_obs_rng = 8.0; /* values are mission dependent */ 

sonar3->min_obs_rng = 8.0; /* units in feet */ 

sonar4->min_obs_rng = 3.0; 

}/* end set_up_sonars() */ 

***************************************************** 

sonars_on() 

**************************************************** j 

void sonars_on(sonarl, sonar2) 

RANGE_DATA * sonarl, *sonar2; 

(. 

via0[DDRB] = 0x3F; /* set data direction regs */ 

via0[DDRA] = 0x00; 

viaO[ORB_lRB] = sonarl ->switch & sonar2->switch; /*turn on sonar */ 

tsleep(l); /* switch debounce time 10ms */ 

}/* end sonars_on() */ 

y*********************** ****************************** 

initialize_sonars() 

**************************************************** j 

void initialize_sonars(sonarl, sonar2) 

RANGE_DATA *sonarl, *sonar2: 

i 
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for ( i = 0; i < 20; ++i) 

viaO[ORB_IRBl = (sonar l->switch & sonar2->switch) I 

sonar l->triggcr I sonar2-> trigger; /* trigger*/ 

viaO[ORB_IRB] = sonarl->switch & sonar2->switch; /* clear */ 

tslecp(5); /* wait for max range return 50 ms*/ 



get_init_avg(sonarl, sonar2); 

}/*end initialize_sonars() */ 

/***************************************************** 

initialize_line() 

**************************************************** j 

void initialize_line(line, loc) 

LINE.SEGMENT *line; 
int loc; 

( 

line->loc = loc; /*set location id # */ 

resctjinc(linc); /* init vars to zero */ 



y****************************** *********************** 

get_init_avg() 

************ **************************************** j 

void get_init_avg(sonarl, sonar2) 

RANGE_DATA *sonarl, *sonar2; 

{ 

int i; 

for(i = 0; i < AVG_PTS; ++i) 

{ 

ping_sonars(sonarl, sonar2); 
tsleep(5); 

sonar l->range_array[i] = (fioat)adc 2(2,0); /* board #1 */ 
sonar2->range_array[i] = (fioat)adc2(3,0); /* board #2 */ 
sonar l->range_total += sonarl->range_array[i]; 
sonar2->range_total += sonar2->range_array[ij; 

} 



sonar l->avg_mg = sonar l->range_total/AVG_PTS; 
sonar2->avg_mg = sonar2->range_total/AVG_PTS; 
sonar l->range_indcx = 0; 
sonar2->range_indcx - 0; 

}/* end get_init_avg() */ 



I ***************************************************** 

ping_sonars() 

**************************************************** j 

void ping_sonars(sonarl, sonar2) 

RANGE_DATA *sonarl, *sonar2; 

( 



via0[ORB_IRB] = (sonar l->switch & sonar2->switch) I sonar l->trigger I 
sonar2->trigger; 

via0[ORB_IRB] = sonar l->switch & sonar2->switch; /* clear */ 

}/* end ping_sonars() */ 
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/***************************************************** 

get_avg_mg() 

**************************************************** j 

void get_avg_mg(sonar) 

RANGE_DATA * sonar; 

{ 

int i; 

if((sonar->switch == SONAR_S\Vl) II (sonar->switch == SONAR_SW2)) 

( 

sonar->raw_rng = adc2(2,0); 

} 

else 

( 

sonar->raw_rng = adc2(3,0); 

) 



fi lter_range_data(sonar); 

}/* end get_avg_mg() */ 

^ ***************************************************** 
lllier_range_daia() 

**************************************************** j 

void filter_range_data(sonar) 

RANGE_DATA * sonar; 

( 

if ((sonar->raw_mg > sonar->avg_mg ) II 

(fabs(sonar->raw_rng - sonar->avg_rng) <= sonar->max_rng_diff) 1 1 
(sonar->bad„mg >= MAX_BAD_PTS)) 

( 

sonar->range_total = sonar->range_toial - 

sonar->range_array[sonar->range_index]; 
sonar->range_array[sonar->range_index] = sonar->raw_mg; 
sonar->range_total += sonar->raw_mg; 
sonar->avg_mg = sonar->range_tolal/AVG_PTS; 
sonar->range_index = (sonar->range_index + 1) % AVG_PTS; 

if(sonar->bad_rng >= MAX_BAD_PTS) 

( 

++sonar->bad_updatcs; 

} 

if(sonar->bad_updates >= MIN_NO_PTS) 

( 

sonar->bad_rng = 0; 

} 

} 

else 

( 

++sonar->bad_rng; 

} 

if((sonar->avg_mg * CONVERT_TO_FEET) <= sonar->min_obs_mg) 

switch sonar->sonar_num 

( 

case 1: 

( 

obstacle_alert = obstacle_alert I 0x8; 
break; 

} 
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case 2: 



( 

obstacle_alcrt = obslacle_alert I 0x4; 
break; 

) 



case 3: 

{ 

obstacle_alert = obstacle_alert I 0x2; 
break; 

} 



case 4: 

( 

obslacle_alert = obstacle_alert I 0x1; 
break; 




}/* end filtcr_rangc_dala() */ 

j ***************************************************** 

update_line_seg() 

**************************************************** j 

void updatc_linc_seg(linc) 

LINE_SEGMENT *line; 

{ 

if(line->range_pt <= M1N_N0_PTS) 

{ 

line_segmcnt_init(linc); 

} 

if(line->range_pt > M1N_N0_PTS) 

{ 

line_seg_compute(line); 
if(line->bad_pt >= MAX_BAD_PTS) 

( 

end_line_segment(line); 

resctjine(line); 

swap_init_points(linc); 

} 




j ***************************************************** 

rcsct_line() 

**************************************************** j 

void reset_line(line) 

LlNE_SEGMENT*line; 

{ 

line->n_s = 0; 
line->i_s = 0; 
line->start_pt_no = 0; 
linc->end_pt_no = 0; 
linc->range_pt = 0; 
line‘>sgmx = 0.0; 
line->sgmy = 0.0; 
line->sgmx2 = 0.0; 
linc->sgmy2 = 0.0; 
line->sgmxy = 0.0; 
linc->sgm_delta_sq = 0.0; 
line->max_x = 0; 
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line->min_x = 0; 



i 

y**^^5»t***5»t************************3»c3jc**5jc*****3»c*3*c****3ic** 

line_segmentjmit() 

Initialize a line segment and its associated variables/llags 
Called f,rom sonar_range() in sonar_bay.c. 

**************************************************** j 

void linc_segmcnt_init(linc) 

LINE.SEGMENT *line; 

( 

/* Read in first points to establish initial line segment */ 
line->line_length = 0.0; 
line->bad_pt = FALSE; 

/* accumulate variables */ 

line->sgmx += Iine->sonar_data_pts[line->n_s][0]; 

line->sgmy += line->sonar_data_pts[line->n_s][l]; 

line->sgmx2 += SQR(line->sonar_data_pts[line->n_sl[0]); 

line->sgmy2 += SQR(line->sonar_data_pts[Iine->n_s][ 1 ]); 

line->sgmxy += line->sonar_data_pts[line->n_s][OJ * 

line->sonar_data_pts[line->n_s][l]; 

line->end_pt_no = line->n_s; 

if(line->sonar_data_pts[line->n_s][OJ < line->sonar_data_pLs[lme->mm_x][0]) 
line->min_x = line->n_s; 

if(line->sonar_data_pts[line->n_s][0] > Iine->sonar_data_pts[line->max_x][0]) 
line->max_x = line->n_s; 

/* Update the counters */ 

if (line->range_pt == 1) 

line->start_pt_no = line->n_s; 

++line->n_s; 

++line->i_s; /* current line segment point counter */ 

if (line->range_pt >= MIN_NO_PTS)/* use x data points for first segment */ 

( 

/* Calculate first line segment values */ 
line->mux = line->sgmx / line->ijs; 
line->muy = line->sgmy / line->i_s; 

line->muxx = line->sgmx2 - (line->sgmx * line->sgmx) / line->i_s; 
line->muyy = line->sgmy2 - (line->sgmy * line->sgmy) / line->i_s; 
line->muxy = line->sgmxy - (line->sgmx * line->sgmy)/ line->i_s; 

line->end _pt_no = line->n_s - 1; 

line->theta_sonar = (atan2((-2.0*line->muxy),(line->muyy-line->muxx))) / 2.0; 

line->r_sonar = line->mux * cos(line->theta_sonar) + Iine->muy * sin(line->lheta_sonar); 
for (line->j_s = 0; line->j_s < MlN_NO_PTS; ++line->j_s) 

( 

line->k_s = (line->j_s + line->start_pt_no); 

line->sgm_delta_sq += SQR(line->sonar_data_pts[line->k_s][0] - line->mux) 

* SQRcos(line->theta_sonar)); 

Iine->sgm_delta_sq += SQR(Iine->sonar_data_pts[line->k_s][l] - line->muy) 

* SQR(sin(line->theta_sonar)); 

line->sgm_delta_sq += 2.0 * (line->sonar_data_pts[line->k_sl[0] - line->mux) 

* (line->sonar_daia_pts[line->k_s][lj - line->muy) 

* cos(line->theta_sonar) * sin(line->theta_sonar); 




f***************************************************** 

Iinc_seg_compuie0 
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Read in subsequent data points, alter a line segment has been 
initialized and more range values are obtained 

**************************************************** j 

line_seg_compute(line) 

LINE SEGMENT * line; 

[ 

/* Calculate test values */ 

line->sigma = line->sgm_delta_sq / line->i_s; 

/* Test new point for linearity fit */ 

line->dclta_line = line->sonar_data__pts[line->n_s][0] * cos(line->thcta_sonar) 

+ line->sonar_dala_pts[line->n_s][l] * sin(line->lheta_sonar) 

- line->r_sonar; 

if (((fabs(line->delta_line)) <= (line->sigma * cl)) II ((fabs(line->delta_line)) <= c2)) 

{ 

line->sgmx += line->sonar_data_pts[line->n_s][0]; 
line->sgmy += linc->sonar_data_pts[line->n_s)[l]; 
line->sgmx2 += SQR(line->sonar_data_pts[line->n_s][0]); 
line->sgmy2 += SQR(linc->sonar_data_pts[line->n_s][l]); 

line->sgmxy += Iine->sonar_data_pts[line->n_s][0] * line->sonar_data_pts[line->n_s][l]; 

linc->mux = line->sgmx / (line->i_s + 1); 

line->muy = linc->sgmy / (line->i_s + 1); 

line->muxx = line->sgmx2 - SQR(line->sgmx) / (line->i_s + 1); 

linc->muyy = linc->sgmy2 - SQR(line->sgmy) / (line->i_s + 1); 

line->muxy = linc->sgmxy - (Iinc->sgmx * line->sgmy) /(line->i_s + 1); 

/* calculate ellipse values */ 

line->m_major = (line->muxx + line->muyy) / 2.0 - sqrt((line->muyy - line->muxx) 

* (line->muyy - line->muxx) / 4.0 + SQR(line->muxy)); 
line->m_minor = (line->muxx + line->muyy) / 2.0 + sqrt((line->muyy - linc->muxx) 

* (line->muyy - line->mu.\x) / 4.0 + SQR(line->muxy)); 
line->d_major = 4.0 * sqrt(fabs(line->m_minor / (line->i_s + 1))); 
line->d_minor = 4.0 * sqrt(fabs(line->m_major / (linc->i_s + 1))); 

/* Test new point for ellipse line->thinncss */ 
if ((line->d_minor / linc->d_major) < c3) 

( 

line->end_pt_no = line->n_s; /* update end point */ 
line->bad_pt = 0; /*reset moving bad_pt counter */ 

/* 

* update line segment parameters to include new 

* point 
*/ 

line->Lheta_sonar = (atan2((-2.0 * linc->muxy), 

(line->muyy - line->muxx))) / 2.0; 
line->r_sonar = line->mux * cos(line->theta_sonar) + linc->muy 

* sin(line->tlieta_sonar); 

line->sgm_delta_sq += 2.0 * (Iine->sonar_data_pts[line->n_s][0] - line->mux) 

* (line->sonar_data_pts[line->n_s][l] - line->muy) 

* cos(line->tiieta_sonar) * sin(line->lheta_sonar); 
line->sgm_delta_sq += SQR(line->sonar_data_pts[line->n_s][l] - line->muy) 

* SQR(sin(line->iheta_sonar)); 

line->sgm_delta_sq += SQR(line->sonar_data_pts[line->n_s][0] - line->mux) 

* SQR(cos(line->iheta_sonar)); 

if(line->sonar_data_pts[line->n_s][0] < line->sonar_data_pis[linc->min_x][0]) 
line->min_x = line->n_s; 

if(line->sonar_data_pts[line->n_s][0] > line->sonar_data_pts[line->max_x][0]) 
line->max_x = line->n_s; 

++linc->n_s; 

++line->i_s; 
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line->delia_x = Iine->sonar_data_pts[line->siart_pt_no][0] 

- Hne->sonar_data_pts[line->end_pt_no][0]; 
line->delta_y = line->sonar_data_pts[line->start_pt_no][ 1 ] 

- line->sonar_data_pts[line->end_pt_no][l]; 
line->linejength = sqrt(SQR(line->delta_x) + SQR(line->deita_y)); 



else 

( 

bad_pt_buffer[line->bad_pt][0] = Iine->sonar_data_pts[line->n_s][0]; 
bad_pt_buffer[line->bad_pt][ 1 ] = line->sonar_data_pts[linc->n_s][lj; 
++Iine->bad_pt; 

} 

} 

else 

( 

bad_pt_buffer[line->bad_pt][0] = Iine->sonar_data_pts[line->n_s][0]; 
bad_pt_buffer[line->bad_ptj[ 1] = line->sonar_data_pts[line->n_s][l]; 
++line->bad_pt; 

} 



^ ***************************************************** 

endJine_segmentQ 

Wrap up a line segment if bad data pt, course change, depth change, 
or segment max length reached 

**************************************************** j 

void cnd_line_segment(line) 

LINE_SEGMENT *line; 

{ 

int i; 

double line_angle; 

/* start new line segment */ 

line->sgmx = 0.0; 
line->sgmy = 0.0; 
line->sgmxy = 0.0; 
line->sgmx2 = 0.0; 
line->sgmy2 = 0.0; 
line->sigma = 0.0; 
line->sgm_delta_sq = 0.0; 

/* close out old segment, convert radius to positive value first */ 
if (line->r_sonar < 0) 

( 

line->theta_sonar = 180 * DEG_TO_RAD + linc->theta_sonar; 
line->r_sonar = -1 * line->r_sonar; 

} 

/* determine start and end points on the computed line segment */ 
line->slart_pt_x = line->sonar_daia_pLs[line->start_pt_no][0]; 
line->start_pt_y = hne->sonar_data_pts[line->start_pt_no][l]; 
line->end_pt_x = Iine->sonar_data_pts[line->end_pt_no][0]; 
line->end_pt_y = line->sonar_data_pts[line->end_pt_no][ 1 ]; 
line->delta_line = line->start_pt_x * cos(line->theta_sonar) 

+ line->start_pt_y * sin(line->theta_sonar) - fabs(line->r_sonar); 
line->start_pt_x = line->start_pt_x - (line->dclta_line * cos(line->theta_sonar)); 
line->start_pt_y = line->start_pt_y - (line->delta Jine * sin(line->theta_sonar)); 
line->delta_line = line->cnd_pt_x * cos(line->theta_sonar)+ line->end_pt_y * sin(linc->thcta_sonar ) 
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- fabs(line->r_sonar); 

line->end_pt_x = linc->end_pt_x - (line->delta_line * cos(line->theta_sonar)); 
line->end_pt_y = line->end_pt_y - (linc->dclla_linc * sin(line->theta_sonar)); 
line->delta_x = line->start_pt_x - line->end_pt_x;- 
line->delta_y = linc->siart_pt_y - linc->end_pt_y; 
line->linejcngth = sqrt(SQR(line->delta_x) + SQR(line->delta_y) 

+ SQR(linc->sonar_data_pis[line->start_pt_no][2] 

- line->sonar_data_pts[line->end_pt_no][2])); 

if((line->line_length >= 24.0) && (line->loc != 1)) 

{ 

match_model(line); 

if(pool_surfacc == 9) /* no match */ 

{ 

crcate_new_obstaclc(linc); 

rccord_line(line); 

ncw_obstaclc = 1 ; /* set flag */ 

} 

} 

++line->n_s; 



create_new_obstacle() 

SicJit************************************************** j 

void create_new_obstacle(line) 

LINE_SEGMENT *line; 

( 

if (line->loc == 4)/* Bottom sonar */ 

{ 

line_angle = atan2(line->sonar_data_pts[line->start_pt_no][2] 

- line->sonar_data_pts [line->end_pt_no] [2] , 

line->sonar_data_pts[line->start_pt_no][OJ - line->sonar_data_pts[line->end_pt_no] [()]); 
line_angle = line_angle - PIOVER2; 

) else 

{ 

line_angle = atan2(line->sonar_data_pts[line->start_pt_no][l] 

- line->sonar_data_pts[line->end_pt_no][ 1], 
sqrt(SQR(line->sonar_data_pts[line->start_pt_no][0] 

- line->sonar_data_pts[line->end_pt_no] [0]) 

+ SQR(linc->sonar_data_pts[line->start_pt_no][2] 

- line->sonar_data_pts[line->end_pt_no][2]))); 

) 

if ((line->loc == 4) /* Bottom sonar */ 

( 

offsetlc = line->sonar_data_pts[line->start_pt_no][3] * 0.087155 * cos(line_angle); 
offsetls = line->sonar_data_pts[line->start_pt_no][3] * 0.087155 * sin(line_angle); 
offset2c = line->sonar_data_pts[line->end_pt_no][3] * 0.087155 * cos(line_anglc); 
offset2s = line->sonar_data_pts[line->end_pt_no][3] * 0.087155 * sin(line_angle); 

) else 

{ 

offsetlc = line->sonar_data_pLs[line->start_pt_no][3] * 0.087155 * cos(line_angle); 
offsetls = line->sonar_data_pts[line->start_pt_no][3] * 0.087155 * sin(line_angle); 
offset2c = line->sonar_data_pLs[line->end_pt_no][3J * 0.087155 * cos(line_angle); 
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offsct2s = line->sonar_data_pLs[line->cnd_pt_no][3] * 0.087155 * sin(line_anglc); 

} 

set_planc_pir(linc); 
linc->n__s = 0; 
linc->start_pt_no = 0; 
linc->cnd__pt_no = 0; 

for (i = 0; i < 100; ++i) 

1 inc- > sonai _daia_pts [ i ] [ 3 ] = 0.0; 



^*********************************************h<******* 

swap_init_points() 

**************************************************** j 

void swap_init_points(linc) 

LINE_SEGMENT *linc; 

{ 

ini i; 

linc->i_s = 0; 

line->rangc_pt = 0; 

for(i = 0; i < line->bad_pt; ++i) 

( 

Iine->sonar_data_pts[linc->n_s][0] = bad_pl_bufTcr[i] [0]; 
line->sonar_data_pts[linc->n_s][l] = bad_pt_buffcr[i][ 1 ]; 

++linc->rangc_pt; 

linc_scgmcntjnit(linc); 

} 

) 

^***************************************************** 

convcrt_coords() 

**************************************************** j 

void convcrt_coords(linc) 

LINE_SEGMENT *linc; 

{ 

Iine->sonar_data_pts[line->n_s][0] = x + avg_mg * cos(psi - 1.57079) 

* CONVERT_TO_FEET; 

linc->sonar_data_pts[line->n_s]1 1] = y + avg_mg * sin(psi - 1.57079) 

* CONVERT_TO_FEET; 

++linc->rangc_pt; 

) 

y***************************************************** 

set_plane_pLr() 

Store the plane data points into the array for display. 

**************************************************** j 

set_planc_pir(linc) 

LINE_SEGMENT *Iinc; 

( 

int i; 

double linc_anglc; 

if(Iine->Ioc == 4) /* bottom sonar */ 

{ 

linc->planc_pts[0][0] = linc->sonar_daLa_ptslline->sum_pl_noJ[0] + offsetlc; 
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Iine->plane_pts[0][l] = line->sonar_data_pts[line->start_pt_no][l]; 
line->plane_pts[0][2] = line->sonar_data_pts[line->start_pt_no][2] + offsetls; 
line->plane_pts[l][0] = Iine->sonar_data_pts[line->start_pt_no][0] - offsetlc; 

line->plane_pts[l][l] = linc->sonar_dala_pts[line->sum_pt_no][l]; 
line->plane_pts[l][2] = line->sonar_data_pts[line->sian_pt_no][2] * offsetls; 
line->plane_pts[2][0] = Iine->sonar_data_pts[line->end_pt_no][0] + offset2c; 

line->planc_pts[2][l] = line->sonar_data_pts[line->end_pt_no][l]; 
line->plane_pts[2][2] = line->sonar_data_pts[line->end_pt_no][2] + offset2s; 
line->plane_pts[3][0] = Iine->sonar_data_pts[line->cnd_pt_no][0] - offset2c; 

line->plane_pts[3][l] = line->sonar_data_pts[line->end_pt_no][l]; 
line->plane_pts[3][2] = line->sonar_data_pts[line->end_pt_no][2] - offset2s; 

} else 



line->plane_pts[0][0] = Iinc->sonar_data_pts[line->start_pt_no][0] 
+ offsetlc * cos(line->theta_sonar); 
line->plane_pts[0] [ 1 ] = line->sonar_data_pts[line->start_pt_no] [2J 
+ offsetls + offsetlc; 

line->plane_pts[0][2] = line->sonar_data_pts[line->start_pt_no][l]; 
line->plane_pts[l][0] = Iine->sonar_data_pts[line->start_pt_no][0] 
+ offsetlc * cos(line->theta_sonar); 
line->plane_pts[l][lj = line->sonar_data_pts[line->start_pt_no][2] 

- offsetls - offsetlc; 

line->plane_pts[l][2] = line->sonar_data_pts[line->start_pt_no][l]; 
line->plane_pLs[2][0] = Iine->sonar_data_pts[line->end_pt_no][0] 

+ offsetlc * cos(line->theta_sonar); 
line->plane_pts[2][ 1] = line->sonar_data_pts[line->end_pt_no][2] 

+ offset2s + offset2c; 

line->plane_pts[2][2] = line->sonar_data_pts[line->end_pt_no][l]; 
line->plane_pts[3][0] = Iinc->sonar_data_pts[line->end_pt_no][0] 

+ offsetlc * cos(line->theta_sonar); 
line->plane_pLs[3][l] = line->sonar_data_pts[line->end_pt_no][2] 

- offset2s * offset2c; 

line->plane_pts[31[2] = line->sonar_data_pts[line->end_pt_no][l]; 

) 

) 



/***************************************************** 
atan2() Not available in GESPAC math lib 

double atan2(xl,y) 
double xl,y; 

( 

double x,xi, q, q2; 
int sign; 

if(y != 0.0) 

( 

x = xl/y; 

else 
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if(xl >= 0.) 

{ 

rciurn(O.O); 

) 

else 

{ 

rcturn(3. 14 1 592654); 

) 

} 

xi = atan(x); 

if((xl < 0.) && (y > 0.)) 

[ 

xi = xi + 3.141592654; 

} 

if((xl < 0.) && (y < 0.)) 

{ 

xi = xi + 3.141592654; 

} 

return(xi); 



/***************************************************** 

init_pool() 

alt*************************************************** j 

void init_pool() 

( 

/* pool [i] [0] = r 
pool[i] [ 1 J = theta 

positions arc taken relative to start point of AUV (xjnit, yjnit)*/ 

poolfO] [0] = y_init; /* north wall - next to launch pt */ 
pool[0][l] = 1.57078; 

pool[l][0] = 117.58 - xjnit; /* de^ep end */ 
pool[l][l] = 0.0; 

pool[2][0] = 60.36 - y_init; 
pool[2][l] = 1.57078; 

pool[3][0] = xjnit; 

pool[3][l ] = 3.141592; /^shallow end */ 

pool[4][0] = 4.0; /*shallow botom */ 
pool[4)[l] = 1.6388; 

pool[5][0] = 8.0; /*deep end */ 
pool[5][l] = 1.57078; 



match_model() 

void match_modcl(lmc) 
LINE_SEGMENT *Iine; 

{ 

int i; 

double delta_r, delia_lheui; 
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pool_surface = 9; f* default for no match */ 
for(i = 0; i < 6; ++i) 

( 

dclta_r = line->r_sonar - pool[il[(V): 
delta_tlicta = fabs(line->theta_sonar) - pool [i][ 1 ] ; 
while(delta_lheta > 3.141592) 

{ 

dclta_thcta = dclta_theta - 3.141592; 

} 

if ((fabs(delta_r) <= max_delta_r) && (fabs(delta_theta) <= max_delta_thcta)) 

{ 

pooLsurface = i; 
return; 



} 

record_line_onO 

void record_line_on() 

{ 

if((outfp2 = fopcn(“obs.d”, “w”)) == 0); /* open fdc */ 

{ 

exit(-l); 




record Jine() 

void rccordjinc(line) 

LINE_SEGMENT *linc; 

( 

mtij; 

for(i =0; i < 4; ++i) 

( 

for(j=0; j < 3; ++j) 

( 

fprintf(outfp2, “%lf”,linc->planc_pts[i][j]); 

} 

fprintf(outfp2, ‘Nn’’); 




/***************************************************** 

recordJine_off() 

void record Jine_off() 

( 

fclose(outfp2); 

) 
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APPENDIX C 



+ ^ + + + + + * + + + ^ + + + + + + + + + + ^ + + + + + + + + * + + + + * + * + + * + + + + + + * + + + ^ + + + + ^ 

Sonar related code for the AUV II simulator in gra' u v/magri no/si m. 

Following is a portion of the header file AUV.H 

********** ************************ ******* ********* **************************** *******/ 
EXT void draw_sonar_plot(); 

EXT void initialize_sonars(); 

EXT void initialize_virtual_pool(); 

EXT void initialize_pool_obs(); 

EXT double offset! c, offset! s, offset2c, offset2s; 

/* structure definitions */ 

typedef struct Poini3Slruct { /* 3D point */ 
double x, y, z; 

} PoinG; 

lypedef PoinG Vccior3; 

typedef SU'uct Matrix4Struct { /* 4 x 4 matrix */ 
double elemeni[4][4]; 

) Matnx4; 

typedef struct { 

float forces[6];/* summation of forces & moments */ 
float mminv[6][6]; /* inverse mass matrix */ 
float acc[6];/* udot, vdot, wdot */ 
float vel[6];/* u,v,w,p,q,r */ 
float pos_change[6]; 
float delta_t;/* time between updates */ 
float pos[3];/* x,y,z */ 
float roll, pitch, heading; 

double commanded_posture[ 15001 [17]; /* designated path for temp use */ 

double waypoint[ 10][4]; 

double psi_0; 

int wpts; 

int obs_avoid; 

int posture_no; 

Matrix H_matrix; 

Mau*ix incremental_H_matrix; 

Matrix T_matrix; 

} DYNAMIC_STRUC, *Dyn_pi r; 

typedef struct Triangle { 

Point3 vO; 

Point3 vl; 
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Point3 v2; 

Vector3 normal; 
int il; 
int i2; 
double d; 
double r; 
double theta; 

) triangle; 

EXT triangle pool [28]: 

typedef struct 

{ /* Data for each sonar’s line segment */ 

double sonar_data_pts[1500][7];/* x,y,z, range, course, depth */ 

double plane_pts[ 100] [4][3]; 

double sgmxx, 

sgmyy, 

sgmxy, 

sgmyx, 

sgmx, 

sgmy, 
r_sonar, 
theta_sonar; 
double mux, 
muy, 

muxx, 
muyy, 

muxy, 
delta_x, 
delta_y, 
sgmx2, 
sgmy2; 

double sgm_delta_sq, 

sigma, 

d__major, 

d_minor, 

m_major, 

m_minor; 

double delta_line, 

start_pt_x, 

start_pt_y, 

end_pt_x, 

end_pt_y; 

double linejcngth; 

int i_s, 

j_s, 

n_s, 

end_pt_no, 

start_pt_no, 

bad_pt, 

too_long, 

too_far_apart, 
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course_change; 

int depth_change, 

rangc_pt, 

n_plane, 

loc; 

) LINE_SEGMENT, *LINE_SEG; 
typcdcf struct 

{ /* Position and orientation of a transducer */ 

Matrix sonar_matnx; 

Point3 ^position; 

Vector3 *direction; 

Vector3 *negate_dir; 
double max_range; 

LINE_S EG M E NT line_data; 

} TRANSDUCER, *SONAR__HEAD; 

typedef struct 

{ /* Set of all transducers */ 

int bottom_on; 
int right_on; 
int left_on; 
int front_on; 

TRANSDUCER bottom_sonar; 

TRANSDUCER right_sonar; 

TRANSDUCER left_sonar; 

TRANSDUCER front_sonar; 

TRANSDUCER top_sonar; 

) SONAR_SUITE, *SONAR_PTR; 

typcdcf struct { 

float bottom_clearance; 
float gravity;/* acceleration due to gravity */ 
float rho; /* water density of environment */ 
float nu; /* water viscosity of environment */ 

AUTOPlLOT_STRUCT autop; 

OBSERVER obs; 

VEHICLE_GEOMETRY geo; /* vehicle geometry struct */ 
FLAGS sys; /* flags struct */ 

COEFFICIENTS coeff; /* hydro coefficients struct*/ 
MBARI_BAY bay; 

DYNAMIC_STRUC dyn;/* ivchicle position struct */ 
AUV_POLYGONS poly;/* auv objects (polygons) */ 
SURFACES surf; 

RECORDER rec; 

NPS.POOL pool; 

PANELS panel; 

SEMAPHORES multi; 

CONSTRAINTS constraint; 

SONAR_SUITE SONAR; 
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double front_range; 



} Submarine, *Sub_ptr; 



/********************************************¥**************************************** 

Sonar specific constants/variables found in SONAR. H 

#include <stdio.h> 

^include <math.h> 



/* Constants for sonar package */ 

#definc MAX_LINE_LENGTH 200.0 
#define MAX_PT_GAP 300.0 
#definc MAX_COURSE_CHANGE 10.0 
#dcfine MAX_SONAR_RANGE 1100.0 
#define MAX_DEFTH_CHANGE 20.0 
#define MIN_NO_PTS 3 
#define MAX.RAND 37767.0 
#dcfine SQR(x) ((x) * (x)) 

#dcfmc CONVERT_TO_lNCHES .2S79 



/* Constants for line seg package * y 

#define cl 3.0 

#define c2 .6 

#define c3 .60 

#define c4 24.0 

typedcf struct 

{ 

double ray_matrix[100][8]; 
int ray_matrix_index; 
Vector3 *ray_origin; 
Vector3 *ray_direction; 
)SONAR_RAY, *RAY_STRUCT; 



/* sigma factor */ 

/* deviation const */ 

/* ellipse thinness ratio */ 

/* minimum line segment length */ 

/* ray-tracing structure */ 






AUV SIMULATOR SONAR SIMULATOR 



This package is called from main_loop.c of auv each time that the auv position is updated. The procedure 
initialize_sonars() is called from the initialize_auv() of initialize.c, it sets all of the sonars to their initial po- 
sitions and orientations. The procedure sonar_range performs a ray-trace routine to determine if a sonar beam 
has intersected with a world polygon in a manner to give a valid sonar return. Sonar__range sends a valid range 
point to the package line_seg_bay-C for least-squares fitting, and determination of a plane through the line that 
has been fit to the data points. The procedure plot_sonar() displays the range points and the planes in the pool.- 
Constants are contained in the sonar.h header file, and structures are in the auv.h file. 






#include <math.h> 
#include <stdio.h> 
#include <maIloc.h> 
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#include <sys/typcs.h> 



#include “auv.h” 

#includc “sonar.h” 

#define NEWTYPE(x) (x *)(malloc((unsigned)sizeof(x))) 

/*** funciion declarations ******/ 

double V3DotO; 

Vector3 *V3New(); 

Vector3 *V3NegateQ; 

Vector3 *V3Duplicate(); 
double V3LengthO; 
double V3SquaredLength(); 

Vector3 *V3Normalize(); 

Vector3 *V3MultByScalar(); 

Vector3 *V3DivideByScalar(); 

Vector3 *V3Sub(); 

Vector3 *V3Add(); 

double V3DistanceBetweenTwcPoints(); 

void V3MultMatrixByPoint(); 

void resetJineO; 

void checkJengthQ; 

void check_distance(); 

void check_course(); 

void check_depih(); 

void ping_sonar(); 

void irace^rayO; 

Vector3 *compute_reflecLion(); 
void get_range(); 

double compute_distance_from_sonarO; 

void initialize_ray_structO; 

void position_sonarO; 

void direct_sonar(); 

void replay_sonar_data(); 

int match_modcl(j; 

initial ize_sonarsO 

Set up the individual sonar head directions, locations, and ranges. 

*************************************************:*** j 

void initial ize_sonars( SONAR_AUV) 

SONAR.SUITE *SONAR_AUV; 

{ 

int i, j,k; 

SONAR_AUV->bottom_on - TRUE; 

SONAR_AUV->bottom_sonar.direction = V3New(0.0, -1.0, 0.0); 
SON AR_AUV->bottom_sonar. position = V3New(0.0, -1.0, 0.0); 
SONAR_AUV->bottom_sonar.line_data.tooJong = FALSE; 
SONAR_AUV->bottom_sonar.line_data.too_far_apart = FALSE; 
SONAR_AUV->bottom_sonar.line_data.course_change = FALSE; 
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SONAR_AUV->botiom_sonar.line_data.depih_change = FALSE; 



SONAR_AUV->right_on = TRUE; 

SONAR_AUV->right_sonar.dircclion = V3New(1.0, 0.0, 0.0); 
SONAR_AUV->right_sonar.posilion = V3Ncw(0.0, -1.0, 0.0); 
SONAR_AUV->right_sonar.line_data.too_long = FALSE; 
SONAR_AUV->right_sonar.linc_data.loo_far_apart = FALSE; 
SONAR_AUV->right_sonar.line_data.course_change = FALSE; 
SONAR_AUV->right_sonar.line_data.depih_change = FALSE; 

SONAR_AUV->left_on = TRUE; 

SONAR_AUV->left_sonar.direction = V3New(-1.0, 0.0, 0.0); 

SON AR_AUV->left_sonar.posi lion = V3New(0.0, -1.0, 0.0); 
SONAR_AUV->left_sonar.line_data.too_long = FALSE; 
SONAR_AUV->left_sonar.line_data.too_far_apart = FALSE; 
SONAR_AUV->left_sonar.line_data.course_change = FALSE; 
SONAR_AUV->left_sonar.line_data.deplh_change = FALSE; 

SONAR_AUV->front_on = TRUE; 

SONAR_AUV->front_sonar.direction = V3New(0.0, 0.0, -1.0); 
SONAR_AUV->front_sonar.position = V3New(0.0, -1.0, 0.0); 
SONAR_AUV->front_sonar.linc_data.too_long = FALSE; 
SONAR_AUV->front_sonar.line_data.loo_far_aparl = FALSE; 
SONAR_AUV->front_sonar.linc_data.coursc_changc = FALSE; 
SONAR_AUV->front_sonar.line_data.depth_change = FALSE; 

SONAR_AUV->bottom_sonar.ncgate_dir = V3Ncw(0.0, 1.0, 0.0); 
SONAR_AUV->right_sonar.negate_dir = V3New(-1.0, 0.0, 0.0); 
SONAR_AUV->left_sonar.negate_dir = V3Ncw(1.0, 0.0, 0.0); 
SONAR_AUV->froni_sonar.negate_dir = V3Ncw(0.0, 0.0, 1.0); 

SONAR_AUV->bottom_sonar.position = V3New(0.0, 0.0, 0.0); 
SONAR_AUV->right_sonar.position = V3New(0.0, 0.0, 0.0); 
SONAR_AUV->left_sonar.posilion = V3Ncw(0.0, 0.0, 0.0); 
SONAR_AUV->front_sonar.position = V3New(0.0, 0.0, 0.0); 

SONAR_AUV->bottom_sonar.line_data.loc = 1; 
SONAR_AUV->right_sonar.line_data.loc = 2; 
SONAR_AUV->left_sonar.line_data.loc = 3; 
SONAR_AUV->front_sonar.line_data.loc = 4; 

SONAR_AUV->bottom_sonar.max_range = MAX_SONAR_RANGE; 
SONAR_AUV->right_sonar.max_range = MAX_SONAR_RANGE; 
SONAR_AUV->left_sonar.max_range = MAX_SONAR_RANGE; 
SONAR_AUV->front_sonar.max_rangc = MAX_SONAR_RANGE; 

for (i=0; i< 100; ++i) 

( 

SONAR_AUV->bottom_sonar.line_data.planc_pts[i][0][0] = 0.0; 
SONAR_AUV->right_sonar.line_data.plane_pts[i][0][0] = 0.0; 
SONAR_AUV->lcft_sonar.linc_data.planc_pts[i][0J[0] = 0.0; 
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SONAR_AUV->front_sonar.line_data.plane_pts[i|[OJ[OJ = 0.0; 




y ******************».*******************:»'************** 

initialize_ray_struct() 

***************************************************** 

void initializc_ray_siruct(ray_siruct) 

SONAR_JRAY *ray_struct; 

( 

int ij; 

for(i = 0; i < 100; ++i) 

( 

for(j = 0; j < 8; ++j) 

{ 

ray_struct->ray_matrix[i][j] = 0.0; 



} 

ray_struct->ray_mairix_index = 0; 
ray_struct->ray_origin = V3New(0.0, 0.0, 0.0); 
ray_sUuct->ray_direction = V3New(0.0, 0.0, 0.0); 



^ ******************** * * * * ************ if.-*. -if. * * 

* draw_sonar_plot 

* Select each sonar in sequence, check for an echo, then plot data. 

* 

**************************************************** j 

void draw_sonar_plot(SONAR_AUV, auv) 

SONAR_SUlTE *SONAR_AUV; 

Sub_ptr auv; 

( 

SONAR_HEAD which_sonar; 

LINE_SEG line_vars; 

volatile SONAR_RAY ray_data; 

volatile SONAR_RAY *ray_struct; 

ray_struct = &ray_data; 

which_sonar = &SONAR_AUV->left_sonar; /* used for mission replay-insert desired sonar*/ 
if (auv->constraint.box) /* actual mission replay switch */ 

{ 

replay_sonar_data(which_sonar, auv); 

} 

else 

( 

initialize_ray_struct(ray_struct); 
which_sonar = &SONAR_AUV->bottom_sonar; 
if (SONAR_AUV->bottom_on) 

( 
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ping_sonar(which_sonar, auv, ray_siruct); 

++ray_slruct->ray_matrix_index; 
which_sonar->dirccLion->y = -0.99619; 
which_sonar->direction->x = 0.07547; 
which_sonar->dircction->z = 0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 
which_sonar->dircction->y = -0.99619; 
which_sonar->dircction->x = 0.07547; 
which_sonar->direction->z = -0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_mairix_index; 
which_sonar->dircciion->y = -0.99619; 
which_sonar->direction->x = 0.0; 
which_sonar->dircction->z = -0.08715; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_maLrix_index; 
which_sonar->dircction->y = -0.99619; 
which_sonar->direction->x = -0.07547; 
which_sonar->direciion->z = -0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 
which_sonar->direction->y = -0.99619; 
which_sonar->direction->x = -0.07547; 
which_sonar->dirccLion->z = 0.04357; 
ping_sonar(which_sonar, auv, ray_slruct); 
++ray_sLruct->ray_matrix_index; 
which_sonar->direclion->y = -0.99619; 
which_sonar->direcLion->x = 0.0; 
which_sonar->direciion->z = 0.08715; 
ping_sonar(which_sonar, auv, ray_siruct); 
++ray_struct->ray_matrix_index; 



which_sonar = &SONAR_AUV->right_sonar; 
if (SON AR_AUV->right_on) 

( 

ping_sonar(which_sonar, auv, ray_struct); 
+ +ray_siruct-> ray _ma tri x_i n dex ; 
which_sonar->direction->x = 0.99619; 
which_sonar->dircciion->y = 0.07547; 
which_sonar->direcuon->z = 0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 
which_sonar->direclion->x = 0.99619; 
which_sonar->dirccuon->y = 0.07547; 
which_sonar->direciion->z = -0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
+ +ray_struc t-> ray _matri x_in dcx ; 
which_sonar->direction->x = 0.99619; 
which_sonar->dirccLion->y = 0.0; 
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which_sonar->direction->z = -0.08715; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_mairix_indcx; 
which_sonar->dircction->x = 0.99619; 
which_sonar->direction->y = -0.07547; 
which_sonar->dircciion->z = -0.04357; 
ping_sonar(which_sonar, auv, ray_.struct); 
++ray_struct->ray_matxix_index; 
which_sonar->direction->x = 0.99619; 
which_sonar->dircciion->y = -0.07547; 
which_sonar->direction->z = 0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_siruct->ray_malrixjndcx; 
which_sonar->dircciion->x = 0.99619; 
which_sonar->dircciion->y = 0.0; 
which_sonar->dircction->z = 0.08715; 
ping_sonar(which_sonar, auv, ray_siruct); 
++ray_struct->ray_matrix_indcx; 



which_sonar = &SONAR_AUV->left_sonar; 
if (SONAR_AUV->lcft_on) 

{ 

ping_sonar(which_sonar, auv, ray_struct); 
++ray_stjuct->ray_matrixjndex; 
which_sonar->dircciion->x = -0.99619; 
which_sonar->dircciion->y = 0.07547; 
which_sonar->dircction->z = 0.04357; 
ping_sonar(which_sonar, auv, ray_siruct); 
++ray_slruct->ray_matrix_indcx; 
which_sonar->dircciion->x = -0.99619; 
which_sonar->direcLion->y = 0.07547; 
which_sonar->dircction->z = -0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_mauix_index ; 
which_sonar->direction->x = -0.99619; 
which_sonar->dircction->y = 0.0; 
which_sonar->direciion->z = -0.08715; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_slruct->ray_malrix_indcx; 
which_sonar->dircction->x = -0.99619; 
which_sonar->dircction->y = -0.07547; 
which_sonar->dircciion->z = -0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_indcx; 
which_sonar->dircciion->x = -0.99619; 
which_sonar->dircction->y = -0.07547; 
which_sonar->dirccuon->z = 0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struci->ray_matrix_indcx; 
which_sonar->dircciion->x = -0.99619; 
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which_sonar->direction->y = 0.0; 
which_sonar->direction->z = 0.08715; 
ping_sonar(which_sonar, auv, ray_siruci); 
++ray_struct->ray_malrix_index; 



which_sonar = &SONAR_AUV->front_sonar; 
if (SONAR_AUV->front_on) 

{ 

ping_sonar(which_sonar, auv, ray_struct); 
++ray_slruct->ray_matrix_index; 
which_sonar->direction->z = -0.99619; 
which_sonar->direction->y = 0.07547; 
which_sonar->direction->x = 0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_sLruct->ray_matrixJndcx; 
which_sonar->dircction->z = -0.99619; 
which_sonar->dircction->y = 0.07547; 
which_sonar->direction->x = -0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 
which_sonar->dircction->z = -0.99619; 
which_sonar->direction->y = 0.0; 
which_sonar->direction->x = -0.08715; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 
which_sonar->dircction->z = -0.99619; 
which_sonar->direciion->y = -0.07547; 
which_sonar->direction->x = -0.04357; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_siruct->ray_mairix_index ; 
which_sonar->dircction->z = -0.99619; 
which_sonar->dircction->y = -0.07547; 
which_sonar->direction->x = 0.04357; 
ping_sonar(which_sonar, auv, ray_siruct); 
++ray_struct->ray_mairixjndex; 
which_sonar->dircciion->z = -0.99619; 
which_sonar->direction->y = 0.0; 
which_sonar->direclion->x = 0.08715; 
ping_sonar(which_sonar, auv, ray_struct); 
++ray_struct->ray_matrix_index; 



which_sonar = &SONAR_AUV->bottom_sonar; 
which_sonar->direction->x = 0.0; 
which_sonar->direction->y = -1.0; 
which_sonar->direction->z = 0.0; 
direct_sonar(which_sonar,auv); 
if (SONAR_AUV->bottom_on) 

{ 

gct_rangc(which_sonar,auv, ray_siruct); 
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) 



which_sonar = &SONAR_AUV->right_sonar; 
which_sonar->direction->x = 1.0; 
which_sonar->direction->y = 0.0; 
which_sonar->direction->z = 0.0; 
direct_sonar(which_sonar,auv); 

if (SONAR_AUV->right_on) 

get_range(which_sonar,auv, ray_siruct); 

} 



which_sonar = &SONAR_AUV->left_sonar; 
which_sonar->direction->x = -1.0; 
which_sonar->dircction->y = 0.0; 
which_sonar->direction->z = 0.0; 
direct_sonar(which_sonar,auv); 

if (SONAR_AUV->left_on) 

{ 

get_range(which_sonar,auv, ray_struct); 

) 



which_sonar = &SONAR_AUV->front_sonar; 
which_sonar->direction->x = 0.0; 
which_sonar->direction->y = 0.0; 
which_sonar->direction->z = -1.0; 
direct_sonar( wh ic h_sonar.au v) ; 
if (SONAR_AUV->front_on) 

{ 

gct_range(which_sonar.auv. ray_struct); 

} 



free(ray_struct->ray_origin); 

frcc(ray_struct->ray_dircction); 

reset_sonars(SONAR_AUV);/* Back to orig posits */ 
} /* end else*/ 

} 



^ ***************************************************** 
plot_sonar() 

Draw range marks and derived planes. Front sonar shows range only. 

**************************************************** j 

plot_sonar(line,auv) 

LINE_SEGMENT *line; 

Sub_ptr auv; 

( 

float tempi [3], temp2[3], temp3[3], temp4[3]; 



89 



int fi; 

/* Plot the range points */ 



for (fi = 0; fi < 1 500; fi = ++fi) 

{ 

if (linc->sonar_data_pts[fi][3] != 0.0) 

{ 

if ((line->loc == 1) II (line->loc ==3)) 

cmov(line->sonar_dataj3ts[fi][0], line->sonar_data_pts[fi][l], 
line->sonar_data_pts[fi][2]); 

else 



/* 

* y & z points swapped by line_seg_bay 

* routines, so swap back 
*/ 

if(line->sonar_data_pts[fi][3] == 1.0) 

( 

RGBcoIor(200, 0,200); 
cmov(line->sonar_data_pts[fi][0], 

1 ine-> sonar_data_pts f fi] [ 1 ] , 
line->sonar_data_pts[fi][2]);} 
else 



cmov(line->sonar_daia_pts[fi][0], 
line->sonar_data_pts[fi] [2] , 
line->sonar_data_pts[fi][l]); 
switch(line->loc) 

( 

case 1: 

RGBcolor(255, 255,0); 
break; 

case 2: 

RGBcolor(0, 255,0); 
break; 

case 3: 

RGBcolor(0, 0, 255); 
break; 



case 4: 

RGBcolor(255, 0, 0); 
break; 




charsU'(“x”); 

if(auv->constraint.box) 

{ 
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cmov(l ine->sonar_data_pts| fi] [ 3] , line->sonar_data_pis[fi ] [4 ] , 
line->sonar_data_pLs[fi][5]); 

charstr(“.”); 



} 

if(auv->constraint.snake) /* LOS guidance path simulator selection*/ 

{ 

cmov(auv->dyn.waypoint[auv->dyn.posturc_no][ 1 ], 
auv->dyn.waypoint[auv->dyn.poslure_no][2], 
-auv->dyn.waypoini[auv->dyn.posture_no][0]); 
charstr(“W”); 

) 

/* Plot the generated planes, sides in white, bottom in black */ 
for (fi = 0; fi < 100; ++fi) 

( 

if (line->plane_pts[fi](0][0] != 0.0) 



if (linc->loc == 1 ) 



RGBcolor(0, 0, 0); 



) else 



RGBcolor(10, 200, 100); 



) 

tempi [0] = (float) 1 
tempi [1] = (float) 1 
tempi [2] = (float) 1 
tcmp2[0] = (float) 1 
temp2[l] = (float) 1 
temp2[2] = (float) 1 
temp3[0] = (float) 1 
temp3[l] = (float) 1 
temp3[2] = (float) 1 
temp4[0] = (float) 1 
tcmp4[l] = (float) 1 
temp4[2] = (float) 1 

bgnpolygon(); 

v3f(templ); 

v3f(temp2); 

v3f(lemp4); 

v3f(temp3); 

endpolygon(); 



ne->plane_pts[fi][0][0] 
nc->plane_pts[fi][0][l] 
ne->planc_pts[fi][0][2] 
ne->plane_pts[fi][l][0] 
ne->plane_pts[fi][l][l ] 
ne->plane_pts[fi][l](2] 
ne->plane_pts| fi ] [2] [0] 
ne->plane_pts[fi][2][l] 
ne->plane_pts[fi] [2] [ 2] 
nc->planc_pts[fi][3][0] 
ne->plane_pts(fi](3][l] 
nc->plane_pts[fi][3][2] 



) 

) 

/***************************************************** 
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sclcci_i 10 & sclect_i2() 

Returns the appropriate value for use in the point-in-polygon test. 
**************************************************** 

select_il(a, b, k) 
double a, b; 
int k; 

( 

switch (pool[k].il) 

( 

case 0: 

return (a); 
break; 

case 1: 

return (b); 
break; 




select_i2(a, b, k) 
double a, b; 
int k; 

( 

switch (pool[k].i2) 

( 

case 1: 

return (a); 
break; 

case 2; 

return (b); 
break; 




j ***************************************************** 

reset_sonars() 

Used to return sonars to original values for next cycle. 
**************************************************** / 



reset_sonars(SONAR_AUV) 

SONAR_SUITE *SONAR_AUV; 

( 

int i j; 

free(SONAR_AUV->bottom_sonar.direction); 

free(SONAR_AUV->right_sonar.direction); 

free(SONAR_AUV->lefl_sonar.direction); 

free(SONAR_AUV->front_sonar.direction); 

free(SONAR_AUV->bottom_sonar.negate_dir); 

free(SONAR_AUV->right_sonar.negate_dir); 



92 



free(SONAR_AUV->left_.sonar.negate_dir); 

frec(SONAR_AUV->front_sonar.negate_dir); 

frec(SONAR_AUV->bottom_sonar.position); 

frec(SONAR„AUV->right_sonar.position); 

frcc(SONAR_AUV->left_sonar.position); 

free(SONAR_AUV->front_sonar.position); 

SONAR_AUV->bottom_sonar.direction = V3New(0.0, -1.0, 0.0); 
SONAR_AUV->bottom_sonar.]ine_data.too_long = FALSE; 
SONAR_AUV->bottom < _sonar.line_data.too_far_apart - FALSE; 
SONAR_AUV->bottom_sonar.line_data.course_change = FALSE; 
SONAR_AUV->bottom_sonar.line_data.depth_change = FALSE; 

SONAR_AUV->right_sonar.direction = V3New(1.0, 0.0, 0.0); 
SONAR_AUV->right_sonar.line_data.too_long = FALSE; 
SONAR_AUV->right_sonar.line_data.too_far_apart = FALSE; 
SONAR_AUV->right_sonar.linc_data.course_changc = FALSE; 
SONAR_AUV->right_sonar.line_data.depth_change = FALSE; 

SONAR_AUV->left_sonar.dircclion = V3New(-1.0, 0.0, 0.0); 
SONAR_AUV->left_sonar.line_data.too_long = FALSE; 
SONAR_AUV->left_sonar.line_data.too_far_apart = FALSE; 
SONAR_AUV->left_sonar.line_data.course_change = FALSE; 
SONAR_AUV->left_sonar.linc_data.depth_change = FALSE; 

SONAR_AUV->front_sonar.direction = V3New(0.0, 0.0, -1.0); 
SONAR_AUV->front_sonar.line_data.too_long = FALSE; 
SONAR_AUV->front_sonar.line_data.too_far_apart = FALSE; 
SONAR_AUV->front_sonar.line_data.course_change = FALSE; 
SONAR_AUV->front_sonar.linc_data.depth_changc = FALSE; 

SONAR_AUV->bottom_sonar.negate_dir = V3New(0.0, 1.0, 0.0); 
SONAR_AUV->right_sonar.negate_dir = V3New(-1.0, 0.0, 0.0); 
SONAR_AUV->left_sonar.negate_dir = V3New(1.0, 0.0, 0.0); 
SONAR_AUV->front_sonar.negate_dir = V3New(0.0, 0.0, 1.0); 
SONAR_AUV->bottom_sonar.position = V3New(0.0, 1.0, 0.0); 
SONAR_AUV->right_sonar.position = V3New(-1.0, 0.0, 0.0); 
SONAR_AUV->left_sonar.posilion = V3New(1.0, 0.0, 0.0); 
SONAR_AUV->front_sonar.position = V3Ncw(0.0, 0.0, 1.0); 



^ *********************;******************************** 
posilion_sonarO 

Using the auv H_matrix, move the sonars to the nose. 

**************^^**^**^***^**^^**^^^ + ^^^^* + *^^^^ 51 .^*^^ I 

void position_sonar(sonar, auv) 

TRANSDUCER *sonar; 

Submarine *auv; 

{ 

Vector3 *tcmp_pi: 
int i,j; 
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pushmatrix(); 

loadmatrix(auv->dyn.H_matrix); 

translate(0.0, O.O, -32.0); 

getmatrix(sonar->sonar_matrix); 

popmatrix(); 

sonar->position->x = sonar->sonar_matrix[3][0]; 
sonar->position->y = sonar->sonar_matrix[3][l]; 
sonar->position->z = sonar->sonar_matrix[3][2]; 



j ***************************************************** 

Orient the sonars depending on heading, pitch, roll. 

************ **************************************** y 

void direct_sonar(sonar, auv) 

TRANSDUCER * sonar; 

Sub_ptr auv; 

{ 

double temp; 

double alpha = auv->dyn. heading * DEG_TO_RAD; 
pushmatrix(); 

loadmatrix(auv->dyn.H_maU'ix); 

getmatrix(sonar->sonar_matrix); 

popmatrix(); 

switch (sonar->line_data.loc) 

( 

case 1: 

V3MultMatrixByPoint(sonar); 
sonar->direction = V3Normalize(sonar->direction); 
sonar->negate_dir->x = -sonar->direction->x; 
sonar->negatc_dir->y = -sonar->direction->y; 
sonar->negate_dir->z = -sonar->direction->z; 
break; 

case 2: 



V3MullMatrixByPoint(sonar); 
sonar->direction = V3Normalize(sonar->direction); 
sonar->direction->z = -sonar->direction->z; 
sonar->negate_dir->x = -sonar->direction->x; 
sonar->negate_dir->y = -sonar->direction->y; 
sonar->negate_dir->z = -sonar->direction->z; 
break; 

case 3: 
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V3MultMaLrixByPoint(sonar); 
sonar->direction = V3NormaIize(sonar->direction); 
sonar->direction->z = -sonar->direclion->z; 
sonar->negate_dir->x = -sonar->direction->x; 
sonar->ncgatc_dir->y = -sonar->dircction->y; 
sonar->negate_dir->z = -sonar->direciion->z; 
break; 

case 4: 

V3MultMaLrixByPoint(sonar); 
sonar->direction = V3NormaIize(sonar->direclion); 
sonar->negate_dir->x = -sonar->direction->x; 
sonar->negate_dir->z = -sonar->direclion->z; 
sonar->negate_dir->y = -sonar->direction->y; 
break; 

case 5: 

sonar->direction->x = sin(auv->dyn.pitch * DEG_TO_RAD) * cos(alpha); 
sonar->direction->y = (fabs(cos(auv->dyn.pitch * DEG_TO_RAD))); 
sonar->direction->z = -(sin(auv->dyn. pitch * DEG_TO_RAD) * sin(alpha)); 
sonar->direction = V3Normalize(sonar->direction); 
sonar->negate_dir->x = -sonar->dircction->x; 
sonar->ncgate_dir->y = -sonar->direction->y; 
sonar->negate_dir->z = -sonar->dirccLion->z; 



) 

I ***************************************************** 

reset Jine() 

void resetjine(line, closest_pt, min_range, auv) 

LINE_SEGMENT *line; 

Point3 *ciosest_pt; 
double min_range; 

Sub_ptr auv; 

{ 

line->n_s = 0; 
line->i_s = 0; 
line->start_pt_no = 0; 
line->end_pi_no = 0; 
line->range_pt = 0; 

line->sonar_data_pts[0][0] = closest_pt->x; 
line->sonar_data_ptsfO][l] = closcsi_pt->y; 
line->sonar_data_pts[0][2] = closest_pt->z; 
line->sonar_data_pts[0][31 = min_range; 
line->sonar_dala_pts[0][4] = auv->dyn. heading; 
line->sonar_data_pts[0][5] = auv->dyn.H_matrix[3][l]; 



/ 



***************************************************** 
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checkJengthO 

**************************************************** j 

void chcck_lcngth(line) 

LINE_SEGMENT *line; 

( 

line->linejength = fsqrt(SQR(line->sonarjiata_pts[line->start_pt_no][0] - 
linc->sonar_data_pts[line->end_pt_no][0]) + 
SQR(linc->sonar_daia_pts[linc->siart_pt_no][lJ - 
line->sonar_data_pts[line->end_pt_no][l]) + 
SQR(line->sonar_data_pts[line->start_pt_no] [2] - 
linc->sonar_data_pts[line->end_pt_no][2])); 

if(line->line Jcngth > MAX_LINE_LENGTH) 
line->too_long = TRUE; 

} 

j ***************************************************** 

chcck_distance() 

**************************************************** j 

void check_distance(line, closest_pt) 

LINE.SEGMENT *line; 

Point3 *closest_pt; 

{ 

double distance_between_pis; 



if (line->loc == 1) 

disiance_between_pLs = fsqrt(SQR(line->sonar_data_pts[line->cnd_pt_no][0] - 

closest_pt->x) + SQR(line->sonar_data_pts[line->end_pLno][l] 
closest_pt->y) + SQR(line->sonar_data_pts[line->end_pt_no][2] 
closest_pt->z)); 

else 

distance_between_pLs = fsqrt(SQR(line->sonar_data_pts[line->end_pt_no][0] - 

closest_pt->x) + SQR(line->sonar_data_pts[line->end_pt_noJ[lJ 
closest_pt->z) + SQR(line->sonar_data_pts[line->end_pt_no][2] 
closest_pt->y )); 

if (distance_between_pts > MAX_PT_GAP) 

{ 

line->too_far_apan = TRUE; 

} 



j ***************************************************** 

check_course() 

**************************************************** j 

void check_course(line, auv) 

LINE_SEGMENT *line; 

Sub_plr auv; 

( 

double heading_difference; 
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heading_difference = line->sonar_data_pts[line->start_pt_no][4] - auv->dyn.heading 
if (heading_difference < 0.0) 

heading_difference = heading_diffcrence + 360.0; 
if (heading_difference > 180.0) 

heading_difference = 360.0 - heading_difference; 

if (heading__difference > MAX_COURSE_CHANGE) 
line->course_change = TRUE; 

} 



y ***************************************************** 

check_depth() 

**************************************************** j 

void check_depLh(line, auv) 

LINE_SEGMENT *line; 

Sub_pir auv; 

{ 

if(fabs(]ine->sonar_data_pts[Iine->siari_pi_no][5] - auv->dyn.H_mairix[3][l ]) > 
MAX_DEPTH_CHANGE) 

]ine->depth_change = TRUE; 

} 

I ***************************************************** 

ping_sonar() 

**************************************************** j 

void ping_sonar(sonar, auv, ray_struct) 

TRANSDUCER * sonar; 

Sub_pir auv; 

SONAR_RAY *ray_siruct; 

{ 

direct_sonar(sonar, auv); 
position_sonar(sonar,auv); 

ray_struct->ray_direction->x = sonar->direction->x; 
ray_struct->ray_direction->y = sonar->direction->y; 
ray_struct->ray_direction->z = sonar->direction->z; 
ray_slruct->ray_origin->x = sonar->position->x; 
ray_slruct->ray_origin->y = sonar->position->y; 
ray_struct->ray_origin->z = sonar- >position->z; 
trace_ray(ray_struct, sonar); 

j 



j ***************************************************** 

Lrace_rayO 

**************************************************** j 

void lrace_ray(ray_sLruct, sonar) 

SONAR_RAY *ray_slruct; 

TRANSDUCER * sonar; 

( 

Vector3 *pool_normaI, *ray_negate_direction, *R; 
int inter, anyjnter, i, g_s, index, node_index; 
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double d_sonar, t, ND, NO, UO, Ul, U2, VO, VI, V2; 

double alpha, beta, distance, retum_angle, cum_range, shortest_distance; 

Point3 *P_inter; 

Point3 *c1osest_pt; 

Point3 *temp_pt; 

P_inter = V3New(0.0, 0.0, 0.0); 
closest_pt = V3New(0.0, 0.0, 0.0); 
temp_pt = V3New(0.0, 0.0, 0.0); 
inter = FALSE; 
any Jnter = FALSE; 

ray_negate_direction = V3New(0.0,0.0,0.0); 

R = V3New(0.0,0.0,0.0); 

ray_negate_direction->x = -ray_struct->ray_direction->x; 
ray_negate_direction->y = -ray_struct->ray_direction->y; 
ray_negate_direction->z = -ray_struct->ray_direction->z; 
index = ray_structo>ray_matrix_index; 
cum_range = 0.0; 
shortest_di stance = 9999.0; 

/* Cycle through all pool polygons in initialize_virtual_pool.c */ 
for (i = 0; i < 28; ++i) 

( 

/* Check for proper reflection angle (> 0 degrees) */ 
return_angle = V3Dot(&pool[i] .normal, ray_negate_dircction); 
if (retum_angle > 0.0) 

( 

ND = V3Dot(ray_struct->ray_direction, &pool[i]. normal); 

NO = V3Dot(ray_stxuct->ray_origin, &pool[i].normal); 

temp_pt->x = -pool[i].v0.x; 

temp_pt->y = -pool[i].v0.y; 

temp_pt->z = -pool[i].v0.z; 

d_sonar = V3Dot(temp_pt, &pool[i].normal); 

t = -1 * ((d_sonar + NO) / ND);/* Quick range check */ 

if ((t > 0.0) && (t <= 2 * MAX_SONAR_RANGE) && (t < shortest.distance)) 

( 

/* Calculate the point of intersection */ 

P_inter->x = ray_struct->ray_origin->x + ray_struct->ray_direction->x * t; 
P_inter->y = ray_struct->ray_origin->y + ray_struct->ray_direction->y * t; 
P_intcr->z = ray_struct->ray_origin->z + ray_struct->ray_di recti on ->z * t; 

U0 = select_il(P_inter->x, P_inter->y, i) - 
select_il(pool[i].v0.x, pool[i].v0.y, i); 

V0 = select_i2(P_inter->y, P_inter->z, i) - 
select_i2(pool[i].v0.y, pool[i].v0.z, i); 

inter = FALSE; 

Ul = select_il(pool[i].vl.x, pool[i].vl.y, i) - 
select_il(pool[i].v0.x, pool[i].v0.y, i); 

U2 = select_il(pool[i].v2.x, pool[i].v2.y, i) - 
seleci_il(pool[i].v0.x, pool[i].v0.y, i); 
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VI = sclcct_i2(pool[i].vl.y, pooI[i].vl.z, i) - 
sclcct_i2(pool|i].v().y, pool[i].v0.z, i); 
V2 = sclcct_i2(pool[i).v2.y, pool[i].v2.z, i) - 
scleci_i2(pool(i).v0.y, pool[i].v0.z, i); 



/* 

* Check to see if point on plane is within 

* polygon 
*/ 

if (U1 == 0) 

{ 

beta = U0 / U2; 

if ((beta >= 0.0) && (beta <= 1.0)) 

{ 

alpha = (VO- beta * V2)/ VI; 

inter = ((alpha >= 0.0) && (alpha + beta <= 1 .0)); 

) 

) 

else 



beta = (V0* U1 - UO * V1)/(V2* U1 - U2 * VI); 
if ((beta >= 0.0) && (beta <= 1.0)) 

{ 

alpha = (U0- beta * U2)/U1; 

inter = ((alpha >= 0.0) && (alpha + beta <= 1.0)); 



if (inter) /* Point was in poly */ 

{ 

any_inier = TRUE; 

distance = V3DisianceBetwccnTwoPoints(P_inter,ray_strucl->ray_origin) 
if (distance < shoriesi_distance) 

{ 

shortest_distance = distance; 
dosest_pt->x = P_inter->x; 
c!osest_pt->y = P_intcr->y; 
closest_pt->z = P_inter->z; 
node_index = i; 

} 




f (any_intcr) 

r 

R = compute_renection(ray_strucu nodejndex); 
ray_slruct->ray_matrix[ index] [0J += shortest_distance; 
cum_range = ray_sLruct->ray_mairix[index][0]; 
ray_struct->ray_matrix[index][l] = cIosest_pt->x; 
ray_siruct->ray_mairix[ index ][2J = closest_pt->y; 
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ray_slruct->ray_malrix[index][3] = closest_pt->z; 
ray_struct->ray_matrix[indcx][4] = R->x; 
ray_struct->ray_maLrix[indcx][5] = R->y; 
ray_struct->ray_matrix[index][6] = R->z; 

++ray_struct->ray_matrix [ index ] [7] ; 

if (ray_struct->ray_matrix [index] [0] < 2 * MAX_SONAR_RANGE) 



/* 

* reset ray 

* origin/direction-reeurse 
*/ 

{ 

++ray_struct->ray_matrix_index; 

ray_struct->ray_matrix_index = ray_slruct->ray_matrix_index % 100; 
index = rayjstruct->ray_matrix Jndex; 
ray_struct->ray_matrix( index] [0] = cum_range; 
ray_struct->ray_direction->x = R->x; 
ray_struct->ray_direction->y = R->y; 
ray_struct->ray_direction->z = R->z; 
ray_struct->ray_origin->x = closest_pt->x; 
ray_struct->ray_origin->y = closest_pt->y; 
ray_struct->ray_origin->z = closest_pt->z; 

ray_struct->ray_matrix[index][7] += ray_struct->ray_matrix[index-l][7]; 
if (ray_struct->ray_matrix[index][7] < 4.0) ^recursion depth*/ 
trace_ray(ray_struct, sonar); 



free(closest_pt); 

free(PJnter); 

free(temp_pt); 

free(ray_negate_direction); 



/***************************************************** 

*compute_reflectionO 

Vector3 *compute_reflection(ray_struct,i) 

SONAR_RAY *ray_struct; 
int i; 



Vector3 *L, *two_N, *R, *temp; 
double LdotN; 

L = V3New(0.0, 0.0, 0.0); 
temp = V3New(0.0, 0.0, 0.0); 

R = V3New(0.0, 0.0, 0.0); 

L = V3Duplicate(rayjstrucL>ray_direction); 
L = V3Negate(L); 

LdotN = V3Dot(L, &pool[i].normal); 

L = V3DivideByScalar(L, LdotN); 
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temp = V3Duplicate(&pool[iJ. normal); 
temp = V3MultByScalar(temp, 2.0); 
temp = VSSutKtemp, L, temp); 

R = V3DividcByScalar( temp, V3Length(temp)); 

free(L); 

free(temp); 

retum(R); 



y***************************************************** 

gct_rangcO 

**************************************************** j 

void get_range(sonar,auv, ray_struct) 

TRANSDUCER * sonar; 

Sub_ptr auv; 

SONAR_RAY *ray_struct; 

( 

double min_range, range_to_pt, angle_between_rays, d, range_of_closcst_pt; 
int i, j, min_found; 

Vector3 *ray_negate_direction; 

LINE_SEGMENT * sonar Jinc; 

Point3 *closest_pt; 

closest_pt = V3New(0.0, 0.0, 0.0); 
range_of_closest_pt = 9999.0; 
min_range = 9999.0; 
min_found = FALSE; 

ray_negate_di recti on = V3New(0.0,0.0,0.0); 
sonar Jine = &sonar->line_data; 

for(i=0; i < 100; ++i) 

( 

if(ray_struct->ray_matrix[i][7] >= 1.0) 

{ 

range_to_pt = V3DistanceBetweenTwoPoints(&ray_struct->ray_matrix[i][l], 

sonar->position); 

if((range_to_pt + ray_$truct->ray_matrix[i][0]) < 2 * MAX_SONAR_RANGE) 

{ 

min_range = (rangc_to_pt + ray_strucL->ray_matiix[i][0])/2; 
if ( min range < range_of_closest_pt ) 

( 

ray_negate_direction->x = ray_struct->ray_mauix[i][l] - sonar->position->x; 
ray_negate_direction->y = ray_struct->ray_matrix[i][2] - sonar->position->y; 
ray_negate_direction->z = ray_struct->ray_matrix[i][3] - sonar->position->z; 
anglc_betwecn_rays = V3Dot(ray_negatc_direction, sonar->dircction); 

if (angle_bcivvcen_rays > 0.99619) /* cos of five degrees */ 

{ 

sonar->line_data.sonar_data_pts[sonar->line_data.n_s][3] = min_range; 
sonar->line_data.sonar_data_pts[sonar->line_data.n_sJ[6] = min_range: 
min_found = TRUE; 
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range_of_closest_pt = min_rangc; 
closest_pi->x = ray_slruct->ray_mairix[ij[l]; 
closest_pt->y = ray_struct->ray_matrix[i][2]; 
cIosest_pt->z = ray_siruct*>ray_matrix[i][3]; 



} 

) 

if (min_found) 

{ 

pushmatrix(); 

loadmatrix(auv->dyn.H_matrix); 
switch (sonar->line_data.loc) 

{ 

case 1: 

translate(0.0, 'Sonar->line_data.sonar_data_pts[sonar->line_data.n_s][3], 
-32.0); 
break; 

case 2: 

translate(sonar->line_data.sonar_data_pis[sonar->line_data.n_s][3],0.0, 
-32.0 ); 

break; 
case 3: 

translate(-sonar->line_data.sonar_data_pts[sonar->line_data.n_s][3],0.0, 
-32.0 ); 

break; 
case 4: 

trans1ate(0.0, 0.0, 

-sonar->line_data.sonar_data_pts[sonar->line_data.n_s][3] - 32.0); 
auv->front_range = 

sonar->Iine_data.sonar_data_pts[sonar->1ine_data.n_s][3]; 

break; 

) 

getmatrix(sonar->sonar_matrix); 

popmatrix(); 

if(matdi_modeI(sonarJine) == TRUE) 

{ 

sonar->line_data.sonar_data_pts[sonar->line_data.n_s][3] = 1.0; 
++sonarJine->n_s; 

sonar_line->n_s = sonar_line->n_s % 100; 

) 



else 



if(sonarJine->range_pt < 2) 

[ 



++sonar_line->n_s; 

sonar Jine->n_s = sonar Jinc->n_s % 100; 

j 

++sonar_line->range_pt; 

} 

sonar->line_data.sonar_data_pts[sonar->line_data.n_s][0] = sonar->sonar_matrix[3][0]; 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s][l] = sonar->sonar_matrix[3][l]; 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s][2] = sonar->sonar_matrix[3][2]; 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s][4] = auv->dyn. heading; 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s][5] = auv->dyn.H_matrix[3][l]; 

if (sonar_line->range_pt > 1) 

{ 

if(sonar->line_data.sonar_data_pts[sonar->line_data.n_s][3] == 1.0) 

{ 

if( sonar Jinc->rangc_pt > MIN_NO_PTS) 

{ 

endj i ne_seg m en t(sonar J i ne) ; 
resetJine(sonar_line,closesi_pt, min_range, auv); 
sonar_line->sonar_data_pts[sonarJine->n_s][3] = min_range; 

} 

else 

i 

resei_line(sonar_line, closest_pt, min_rangc, auv); 

sonar Jine->sonar_daia_pLs[sonar_linc*>n_s][3] = min_range; 

) 

} 

else 

( 

check_course(sonar_line, auv); 
chcck_depih(sonar_linc, auv); 

) 



if((sonar Jine->too_long == TRUE) II (sonar_line->course_change == TRUE) 

II (sonar_line->depth_change == TRUE) II (sonar _line->too_far_apart == TRUE)) 



il (sonar Jine->rangc_pt > MIN_NO_PTS) 

{ 

end_line_segment(sonar_line); 
rcsctJine(sonar_line,closcst_pi, min_range, auv); 
sonar_line->sonar_data_pts[sonar_line->n_s][3] = min_range; 
linc_segmeni_inii(sonar); 

) 

else 



reseiJine(sonar_line, closest_pt, min_range, auv); 
sonar_linc->sonar_data_pts[sonar_linc->n_s][3] = minrange; 
1 i ne_seg m en t_i n i i(sonar) ; 



else 
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++sonar_line->range_pt; 
if (sonar_line->loc != 5) 

if (sonar_line->range_pt <= MIN_NO_PTS) 

l 

line_segmentJnit(sonar); 

} 

if (sonar _linc->range_pt > MIN_NO_PTS) 
line_seg_computc(sonar); 



free(ray_negate_direciion); 

free(closest_pt); 

) 



^***************************************************** 

match_modclO 

**************************************************** j 

int match_modcl(sonar_line) 

LINE_SEGMENT *sonar_line; 

( 

int match, i; 
match = 0; 

for(i = 0; i < 28; ++i) 

( 

if((fabs(sonar_line->r_sonar * pool[i].r) <= MAX_DELTA_R) && 

(fabs(sonar_line->thcia_sonar - pool [i]. theta) <= MAX_DELTA_THETA)) 

( 

match = 1; 




retum(match); 

} 



y ***************************************************** 
replay_sonar() 

**************************************************** j 

void replay_sonar_data(sonar, auv) 

TRANSDUCER *sonar; 

Sub_ptr auv; 

t 

double SIN_5, raw_rng; 

SIN_5 = .087155; 

raw_mg = auv->dyn.commanded_posture[auv->dyn.posture_no][14]; 
pushmatrix(); 

loadmatrix(auv->dyn.H_matrix); 
rotate(-(Angle)auv->dyn.pos_change[5], ‘y’); 
rotate( (Angle)auv->dyn.pos_change[4], ‘x’); 
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rotate(-(Angle)auv->dyn.pos_change[3], V); 



switch (sonar->line_data.loc) 



case 1: 

translate(0.0, -raw_mg * CONVERT JTOJNCHES, -32.0); 
break; 
case 2: 

translatc(raw_mg * CONVERT JTOJNCHES, 0.0, -32.0); 
break; 
case 3: 

if(fabs(auv->dyn.commanded_posture[auv->dyn.posture_no][9]) > 0.04357) 
raw_mg += raw_rng * SIN_5; 
translate(-raw_mg * CONVERT JTOJNCHES, 0.0, -32.0 ); 
break; 

case 4: 

translate( 0.0, 0.0, -raw.rng * CONVERT_TO_INCHES - 32.0); 
break; 

) 

getmatrix(sonar->sonar_matrix); 

popmatrix(); 



sonar->line_data.sonar_data_pts[sonar->line_data.n_s][0] = sonar->sonar_matrix[3][0]; 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s] [ 1 ] = sonar->sonar_matrix[3] [ 1 ] ; 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s][2] - sonar->sonar_matrix[3]l2]; 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s][3] = auv->dyn.H_rnatrix[3][0]; 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s][4] = auv->dyn.H_matrix[3][l]; 
sonar->line_data.sonar_data_pts[sonar->line_data.n_s][51 = auv->dyn.H_matrix[3][2]; 
++sonar->line_data.n_s; 

sonar->line_data.n_s = sonar->line_daia.n_s % 1500; 



Vector library routines from “GRAPHICS GEMS”, edited by Glassner [Ref. 26] 

/* return the dot product of vectors a and b */ 
double V3Dot(a, b) 

Vector3 *a, *b; 

{ 

return ((a->x * b->x) + (a->y * b->y) + (a->z * b->z)); 

) 



Vector3 *V3Negate(v) 
Vector 3 *v; 

{ 

v->x = -v->x; 
v->y = -v->y; 
v->z = -v->z; 
return (v); 

) 
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/* return vector sum c = a + b */ 

Vecior3 * V3Add(a, b, c) 

Vector3 *a, *b, *c; 

{ 

c->x = a->x + b->x; 
c->y = a->y + b->y; 
c->z = a->z + b->z; 
rcturn(c); 

) 

/* create, initialize and return a new vector */ 
Vector3 * V3New(a, b, c) 
double a, b, c; 

{ 

Vector3 *v = NEWTYPE(Vector3); 

v->x = a; 
v->y = b; 
v->z = c; 
return (v); 

) 



/* scales the input vector to the new length and returns it */ 
Vector3 *V3MultByScalar(v, scalar) 

Vector3 *v; 
double scalar; 

( 

v->x *= scalar; 
v->y *= scalar; 
v->z *= scalar; 
return(v); 

) 

/* return vector difference oc = a - b */ 

Vector3 * V3Sub(a, b, c) 

Vector3 *a, *b, *c; 

{ 

c->x = a->x - b->x; 
c->y = a>>y - b->y; 
c->z = a->z - b->z; 
retum(c); 

} 



/* divides the vector by a scalar and returns it */ 
Vector3 *V3DivideByScalar(v, scalar) 

Vector3 *v; 
double scalar; 

{ 

if (scalar != 0.0) 

{ 
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v->x = v->x/ scalar; 
v->y = v->y/scalar; 
v->z = v->z/scalar; 
retum(v); 



} 

/* returns squared length of input vector */ 
double V3SquaredLength(a) 

Vector3 *a; 

{ 

retum(SQR(a->x) + SQR(a->y) + SQR(a->z)); 

} 



/* returns length of input vector */ 
double V3Length(a) 

Vector3 *a ; 

( 

retum(sqrt(V3SquaredLength(a))); 

} 



/* normalizes the input vector and returns it */ 
Vector3 *V3Normalize(v) 

Vector3 *v; 

( 

double len = V3Lengih(v): 
if (len != 0.0) 

( 

v->x /= len; 
v->y /= len; 
v->z /= len; 
retum(v); 

} 

) 

/* create, initialize, and return a new vector */ 
Vector3 *V3Duplicate(a) 

Vector3 *a; 

{ 

Vector3 *v = NEWTYPE(Vector3); 

v->x = a->x; 

v->y = a->y; 

v->z = a->z; 

retum(v); 

) 

/* return the distance betwqeen two points */ 
double V3DistanceBetweenTwoPoints(a, b) 
Point3 *a, *b; 

{ 

double dx = a->x - b->x; 
double dy = a->y - b->y; 
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double dz = a->z - b->z; 
retum(sqrt(SQR(dx) + SQR(dy) + SQR(dz))); 



) 

void V3MullMairixByPoint(sonar) 

TRANSDUCER *sonar; 

{ 

Vector3 *temp_pt; 

temp_pt = V3New(0.0, 0.0, 0.0); 

temp_pt*>x = sonar->sonar_matrix[0][0] * sonar->direction->x + 
sonar->sonar_matrix[0][l] * sonar->direction->y + 
sonar->sonar_matrix[0][2] * sonar->directbn->z; 
temp_pt->y = sonar->sonar_matrix[l][0] * sonar->direction->x + 
sonar->sonar_matrix[l][l] * sonar->direction->y + 
sonar->sonar_matrix[l][2] * sonar->direction->z; 
tcmp_pt->z = sonar->sonar_mauix[2][0] * sonar->direction->x + 
sonar->sonar_matrix[2][l] * sonar->direction->y + 
sonar->sonar_matrix[2][2] * sonar->direction->z; 
sonar->direction->x = temp_pt->x; 
sonar->direction->y = temp_pt->y; 
sonar->direction->z = temp_pt->z; 

} 



This package takes individual range points from sonar_bay.c and 
computes a least-squares fit line for the points. The line is ended 
for various reasons and a plane is fit to the line, based on the 
computed end points and the width is determined by the range from the 
sonar. Points and planes are passed back to sonar_bay.c for plotting 



^include <stdio.h> 

#include <math.h> 

#include “auv.h” 

#include “sonar.h” 

/***************************************************** 

line_segmenl_init() 

Initialize a line segment and its associated variables/llags 
Called from sonar_range() in sonar_bay.c. 

line_segment_init(sonar) 

TRANSDUCER *sonar; 

( 

LINE_SEGMENT *line; 

line = &sonar->line_data; 
if ((line->loc != 1) && (line->loc != 5)) 
convert_coords(line); 
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/* Read in first points to establish initial line segment */ 
line->line_length = 0.0; 
linc->bad_pt = FALSE; 

/* accumulate variables */ 
line->sgmx += line->sonar_dala_pts[line->n_s]10]; 
line->sgmy += line->sonar_data_pts[line->n_s]1 1]; 
line->sgmx2 += SQR(linc->sonar_data_ptsfline->n_s][OJ); 
line->sgmy2 += SQR(line->sonar_data_pLs[line->n_s]| 1]); 

line->sgmxy += Iine->sonar_data_pts[line->n_s][0] * line->sonar_data_pts[line->n_s][ll; 
line->end_pt_no = line->n_s; 

/* Update the counters */ 
if (line->n_s == 99) 
line->n_s = 0; 
else 

++line->n_s; 

if (line->range_pt == 0) 

line->start_pt_no = line->n_s; 

++line->i_s;/* current line segment point counter */ 



if (line->range_pt == M1N_N0_PTS)/* use x data points for first segment */ 

{ 

/* Calculate first line segment values */ 
line->mux = line->sgmx / line->i_s; 
line->muy = line->sgmy / line->i_s; 

line->muxx = line->sgmx2 - (line->sgmx * line->sgmx) / linc->i_s; 
line->muyy = line->sgmy2 - (line->sgmy * line->sgmy) / line->i_s; 
line->muxy = line->sgmxy - (line->sgmx * line->sgmy)/ line->i_s; 

line->end_pt_no = line->n_s - 1; 
if (line->end_pt_no < 0) 

line->end_pt_no = line->end_pt_no + 100; 

line->theta_sonar = (atan2(-2.0 * line->muxy, (line->muyy - line->muxx))) / 2.0; 
line->r_sonar = line->mux * cos(line->theta_sonar) + line->muy * sin(linc->theta_sonar); 

for (line->j_s = 0; line->j_s < MlN_NO_PTS; ++line->j_s) 

{ 

line->j_s = (line->j_s + line->start_pt_no) % 100; 

line->sgm_delta_sq += SQR(line->sonar_data_pis[line->j_s][0] - line->mux) 

* SQR(cos(line->theta_sonar)); 

line->sgm_delta_sq += SQR(line->sonar_data_pLs[line->j_s][l] - line->muy) 

* SQR(sin(line->theta_sonar)); 

line->sgm_delta_sq += 2.0 * (line->sonar_data_pLs[line->j_s][0] - line->mux) 

* (line->sonar_data_ptsIline->j_s]f 1] - line->muy) 

* cos(line->theta_sonar) * sin(line->theta_sonar); 

} 
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} 



} 



I***************************************************** 

line_seg_computc(J 

Read in subsequent data points, after a line segment has been 
initialized and more range values are obtained 



line_seg_compute(sonar) 

TRANSDUCER *sonar; 



{ 

LINE_SEGMENT *line; 

/* line &= sonar->line_data; */ 
line = &sonar->line„data; 

if ((line->loc != 1) && (line->loc != 5)) 
convert_coords(line); 



/* Calculate test values */ 

line->sigma = line->sgm_delta_sq / linc->i_s; 

/* Test new point for linearity fit */ 

line->delta_line = line->sonar_data_pts[line->n_s][0] * cos(line->theta_sonar) 

+ linc->sonar_data_pts[line->n_s][l] * sin(line->thcta_sonar) 

- line->r_sonar; 

if ((fabs(line->delta_line) < (line->sigma * cl)) II (fabs(line->delta_line) < c2)) 

{ 

line->sgmx += line->sonar_data_pts[line->n_s][0]; 
line->sgmy += line->sonar_data_pts[line->n_s][l]; 
line->sgmx2 += SQR(line->sonar_data_pts[line->n_s][OJ); 
line->sgmy2 += SQR(line->sonar_data_pts[linc->n_s][l]); 

line->sgmxy += Iine->sonar_data_pts[line->n_s][0] * line->sonar_data_pts[line->n_s][l]; 

line->mux = line->sgmx / (line->i_s + 1); 

line->muy = line->sgmy / (line->i_s + 1); 

line->muxx = line->sgmx2 - SQR(line->sgmx) / (line->i_s + 1); 

line->muyy = line->sgmy2 - SQR(line->sgmy) / (line->i_s + 1); 

line->muxy = line->sgmxy - (line->sgmx * line->sgmy) / (line->i_s + 1); 

/* calculate ellipse values */ 

line->m_major = (line->muxx + line->muyy) /2.0 - sqrt((line->muyy - line->muxx) 

* (linc->muyy - linc->muxx) / 4.0 + SQR(line->muxy)); 
line->m_minor = (line->muxx + line->muyy) / 2.0 + sqrt((line->muyy - line->muxx) 

* (line->muyy - line->muxx) /4.0 + SQR(line->muxy)); 
line->d_major = 4.0 * sqrt(fabs(line->m_minor / (line->i_s + 1))); 
line->d_minor = 4.0 * sqrt(fabs(line->m_major / (line->i_s + 1))); 
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/* Test new point for ellipse line->thinness */ 
if ((line->d_minor / line->d_major) < c3) 

{ 

line->end_pMio = linc->n_s;/* update end point */ 

/* 

* update line segment parameters to include new 

* point 
*/ 

line->thcta_sonar = (atan2(-2.0 * line->muxy, (line->muyy - line->muxx))) / 2.0; 
line->r_sonar = line->mux * cos(line->theta_sonar) + line->muy 

* sin(line->theta_sonar); 

line->sgm_delta_sq += 2.0 * (line->sonar_data_pls[line->n_s][0] - line->mux) 

* (line->sonar_data_pts[line->n_s][l] - line->muy) 

* cos(line->theta_sonar) * sin(line->theta_sonar); 
line->sgm_delta_sq += SQR(line->sonar_data_pLs[line->n_s][l] - linc->muy) 

* SQR(sin(line->theta_sonar)); 

line->sgm_delta_sq += SQR(line->sonar_data_pLs|line->n_s][0] - line->mux) 

* SQR(cos(line->theta_sonar)); 

if (line->n_s == 99) 
line->n_s = 0; 
else 

++line->n_s; 

++line->i_s; 

line->delta_x = line->sonar_data_pts[line->start_pt_no][0] 

- Iine->sonar_data_pts[line->end_pi_no][0]; 
line->delta_y = line->sonar_data_pts[line->start_pt_no][l] 

- line->sonar_data_pts[line->end_pt_no][l]; 
line->line_length = sqrt(SQR(line->delta_x) + SQR(line->delta_y) 

+ SQR(line->sonar_data_pts[line->start_pt_no][3] 

- line->sonar_data_pts[line->end_pt_no][3])); 

/*if (line->line_length > MAX_LINE_LENGTH) 

{ 

line->too_long = TRUE; 
end_line_segment(line); 

) 

chcck_for_termination(line);*/ 

) else 

( 

line->bad_pt = TRUE; 
cnd_line_segmentOine); 



} else 

l 

line->bad_pt = TRUE; 
end J ine_segment(l ine); 

) 



in 



! ***************************************************** 
endJinc_segmcntO 

Wrap up a line segment if bad data pt, course change, depth change, 
or segment max length reached. 

sic*************:************************************** / 



end_line_segment(line) 

LINE_SEGMENT*line; 



int i; 

double line_angle; 

if ((line->bad_pt == TRUE)) 

{ 

/* start new line segment */ 
line->sgmx = Iine->sonar_data_pts[line->n_s][0]; 
line->sgmy = line->sonar_data_pts[line->n_s][l]; 
line->sgmx2 = SQR(line->sonar_data_pts[line->n_s][0]); 
line->sgmy2 = SQR(line->sonar_data_pts[line->n_s][l]); 

line->sgmxy = Iinc->sonar_data_pts[line->n_s][0] * line->sonar_data_pts[line->n_s][l]; 

line->i_s = 1; 

line->sigma = 0; 

linc->sgm_delta_sq = 0; 

line->range_pt = 1; 

) else 

line->range_pt = 0; 

/* close out old segment, convert radius to positive value first */ 
if (line->r_sonar < 0) 

{ 

line->theta_sonar = 180 * DEG_TO_RAD + line->theta_sonar; 
line->r_sonar = -1 * line->r_sonar; 

) 

/* determine start and end points on the computed line segment */ 
line->start_pt_x = Iine->sonar_data_pts[line->start_pt_no][0]; 
line->start_pt_y = line->sonar__data_pts[line->start_pLno][l]; 
line->end_pt_x = Iine->sonar_daia_pts[line->end_pt_no][0]; 
line->end_pt_y = line->sonar_data_pts[line->end_pt_no][l]; 
line->delia_line = line->start_pt_x * cos(line->theta_sonar) + line->start_pt_y 
* sin(line->theta_sonar) - fabs(line->r_sonar); 
line->start_pt_x = linc->start_pt_x - (line->dclta_line * cos(line->theta_sonar)); 
line->start_pLy = line->start_pt_y - (line->delia_line * sin(line->theta_sonar)); 
line->delta_line = linc->end_pt_x * cos(linc->theta_sonar) + line->cnd_pt_y * sin(line->theta_sonar) 
- fabs(line->r_sonar); 

line->end_pt_x = line->end_pt_x - (line->delta_line * cos(line->theta_sonar)); 
line->cnd_pt_y = line->end_pt_y - (linc->deltajinc * sin(linc->theta_sonar)); 
line->delia_x = line->start_pt_x - line->end _pt_x; 
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line->delta_y = line->start_pt_y - line->end_pt_y; 
line->line_length = sqri(SQR(linc->dclta_x) + SQR(line->delta_y) 

+ SQR(line->sonar_dala_pts[line->start_pt_no][2] 

- line->sonar_data_pts[line->cnd_pt_no][2])); 

if (line->loc == 1 )/* Bottom sonar */ 

( 

line_angle = atan2(line->sonar_data_pts[line->start_pt_no][2] 

- 1 ine->sonar_da ta_pts[ 1 inc->cnd_pt_no] [ 2] , 
line->sonar_data_pts[line->start_pt_no][0] 

- Iine->sonar_data_pts[line->end_pt_no][0]); 
linc_anglc = linc_anglc - PIOVER2; 

} else 

{ 

line_angle = atan2(line->sonar_data_pts[line->stari_pt_no][l] 

- line->sonar_daia_pis[line->end_pt_no][ 1 ], 
sqrt(SQR(line->sonar_daia_pts[line->start_pt_no][0] 

- Iine->sonar_data_pts[line->end_pt_no][0]) 

+ SQR(line->sonar_data_pts[line->slart_pt_no][2] 

- line->sonar_data_pts[line->end_pt_no][2]))); 

} 

if ((line->loc == 1) II (linc->loc == 5))/* Bottom sonar */ 

{ 

offsetlc = line->sonar_data_pts[line->start_pt_no][3] * 0.0871548 * cos(line_anglc); 
offsetls = line->sonar_data_pts[line->start_pt_no][3] * 0.0871548 * sin(line_angle); 
offset2c = line->sonar_data_pts[line->end_pt_no][3] * 0.0871548 * cos(line_angle); 
offset2s = line->sonar_data_pts[line->end_pt_no][3] * 0.0871548 * sin(line_angle); 

} else 



offsetlc = line->sonar_data_pLs[line->stari_pi_no][3] * 0.0871548 * cos(line_angle); 
offsetls = line->sonar_data_pts[line->start_pt_no][3J * 0.0871548 * sin(line_angle); 
offset2c = line->sonar_data_pts[line->end_pt_no][3] * 0.0871548 * cos(line_angle); 
offset2s = line->sonar_data_pts[line->end_pt_no][3] * 0.0871548 * sin(line_angle); 



if ((line->line_length >24.0) && (line->loc != 4)) 

{ 

set_plane_ptr(line); 
if (line->n_plane < 99) 

++line->n_plane; 

else 

line->n_plane = 0; 



line->n_s = 0; 
line->start_pt_no = 0; 
line->end_pt_no = 0; 

for (i = 0; i < 100; ++i) 

line->sonar_data_pts[i][3] = 0.0; 



if(offsetlc > 48.0) /* limit width of planes to four feet */ 



offsctlc = 48.0; 
if(off sells > 48.0) 
offsetls = 48.0; 
if(offsct2c > 48.0) 
offset2c = 48.0; 
if(offsct2s > 48.0) 
offsets = 48.0; 



} 



y ****** *********************************************** 

conven_coords() 

**************************************************** j 

c on vert_coord$(li ne) 

LINE.SEGMENT *line; 



{ 

double temp_val: 

/* 

* store z data into y var, y into z for bottom sonar 
*/ 

temp_val = linc->sonar_data_pts[line->n_s][2]; 
line->sonar_data_pts[line->n_sl[2] = line->sonar_data_pts[line->n_s][l] 
line->sonar_data_pts[line->n_s][l] = temp_val; 

} 



^ ***************************************************** 
check_for_tcrrmination() 

**************************************************** j 

check_for_termination(line) 

LINE_SEGMENT *line; 

t 

if (sqrt(SQR(line->sonar_data_pts[linc->start_pt_no][0] 

- Iine->sonar_data_pts[line->end_pt_no][0]) 

+ SQR(line->sonar_daia_pis[line->start_pl_no][l] 

- 1 ine->sonar_data_pts[line->end_pt_no] [ 1 ] ) 

+ SQR(line->sonar_data_pus[line->siart_pt_no][2] 

- line->sonar_daia_pts[linc->cnd_pi_no][2])) 

> MAX_LINE_LENGTH) 

{ 

line->end_pt_no = line->end_pt_no - 1; 
line->too_long = TRUE; 



) 

if ((line->loc == 1) II (line->loc == 4)) 

{ 

if (fabs(linc->sonar_data_pts[line->start_pt_no][4] 
- linc->sonar_data_pts[linc->end_pt_no][4]) 
> MAX_COURSE_CHANGE) 
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{ 

line->course_change = TRUE; 

} 

} else 



if (fabs(line->sonar_data_pts[line->start_pt_no]f4] 
- line->sonar_data_pts[line->end_pt_no][4]) 
> M AX_DEPTH_CH ANG E) 

( 

line->depth_change = TRUE; 



} 

) 

if ((line->too_long) II (line->course_change) II (line->depth_change)) 
end_line_segment(line); 



y***************************************************** 

set_plane_ptr() 

Store the plane data points into die array for display. 

**************************************************** j 

set_plane_ptr(line) 

LINE_S EG M E NT *line; 

{ 

int i; 

double line_angle; 

if ((line->loc == 1) II (line->loc == 5)) 

( 

line->planc_pLs[line->n_plane]fO][0] = Iine->sonar_data_ptsfline->start_pt_no][0] 4- offsetlc: 
line->plane_pts[line->n_plane][0][l] = line->sonar_data_pts[line->start_pt_no][l]; 
line->plane_pts[linc->n_plane][0][2] = line->sonar_data_pts[linc->start_pt_no][2] +offsetls; 
line->plane_pLs[line->n_planeJ[l][0] = Iine->sonar_data_pts[line->start_pt_no][0] - offsetlc; 

line->plane_pts[line->n_plane][l][l] = line->sonar_data_pts[line->start_pt_no][l]; 
line->plane_pLs[line->n_planel[l][2] = line->sonar_data_pts[linc->start_pt_no][2] -offsetls; 
line->plane_pts[line->n_plane][2][0] = Iine->sonar_data_pts[line->end_pt_no][0] + offset2c; 

linc->planc_pls[line->n_planc][2][l] = line->sonar_data_pts[line->end_pt_no][l]; 
line->plane_pls[line->n_plane][2][2] = linc->sonar_data_pts[line->end_pt_no][2] + offsct2s; 
line->plane_pts[line->n_plane][3][0] = linc->sonar_data_pts[line->end_pt_no][0] - offset2c; 

line->plane_pLs[line->n_pIane][3][l] = line->sonar_data_pLs[line->end_pt_no][l ]; 
line->plane_pts[line->n_plane][3][2] = line->sonar_data_pts[line->end_pt_no][2] - offsct2s; 

} else 



line->plane_pts[line->n_plane][0][0] = line->sonar_data_pts[line->start_pt_no][0] 

+ offsetlc * cosdine->theta_sonar); 

Iine->plane_pts[line->n_plane][0][l] = line->sonar_data_pts[line->start_pt_no][2] + offset Is 
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+ offsetlc; 

line->plane_pls[line->n_planc][0][2] = line->sonar_data_pts[line->start_pt_no][ 1]; 
line->plane_pts [line->n_plane] [ 1 ] [0] = line->sonar_data_pts[ line->start_pt_no] [0] 
+ offsetlc * cos(line->theta_sonar); 

line->planc_pLs[linc->n_planc][l][lj = line->sonar_data_pLs[linc->siart_pi_no][2J 

- offsetls - offsetlc; 

line->plane_pls[line->n_plane][l][2] = line->sonar_data_pts[line->start_pLno][l]; 
line->plane_pts[linc->n_plane][2][0] = Iine->sonar_data_pts[line->end_pt_no][0] 

+ offsetlc * cos(line->theta_sonar); 

line->plane_pls[line->n_plane] [2][1] = line->sonar_data_pts[line->end_pt_no] [2] 

+ offset2s + offset2c; 

line->plane_pts[line->n_plane][2][2] = line->sonar_data_pts[line->end_pt_no][l]; 
line->plane_pis[line->n_plane][3][0] = Iine->sonar_data_ptsfline->end_pt_no][0] 

+ offsetlc * cos(line->theta_sonar); 

line->plane_pts[line->n_plane][3][l] = line->sonar_data_pts[line->end_pt_no][2] 

- offset2s - offset2c; 

line->plane_pts[line->n_plane][3][2] = line->sonar_data_pts[line->end_pt_no][l]; 

) 

} 
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